本發(fā)明涉及船舶動力定位控制領(lǐng)域，特別涉及一種基于自適應(yīng)擴(kuò)展卡爾曼濾波算法的動力定位自抗擾控制方法。

背景技術(shù)：

1、隨著海洋工程的不斷發(fā)展，人類不再局限于陸地資源，而是逐漸將目光轉(zhuǎn)向豐富的海洋資源。海上作業(yè)的復(fù)雜程度日益增高，面臨的環(huán)境干擾更加復(fù)雜多變，對船舶定位能力的要求也在不斷提高。動力定位利用船舶自身的推進(jìn)裝置產(chǎn)生抵抗海洋環(huán)境干擾的推力，從而實(shí)現(xiàn)在海面上的定點(diǎn)懸停。然而動力定位船在海上作業(yè)時(shí)會受到復(fù)雜海洋環(huán)境影響，使船舶無法準(zhǔn)確定位到期望位置，引起動力定位控制器不斷改變輸出控制力，從而導(dǎo)致執(zhí)行機(jī)構(gòu)磨損嚴(yán)重，大大縮減執(zhí)行機(jī)構(gòu)使用壽命，因此對于動力定位的自抗擾控制是十分重要的。

2、目前船舶動力定位自抗擾控制主要采用算法整定pid參數(shù)、跟蹤微分器與狀態(tài)觀測器結(jié)合的方法實(shí)現(xiàn)，但是基于pid控制方法的船舶動力定位自抗擾控制方法參數(shù)調(diào)節(jié)復(fù)雜，導(dǎo)致難以平衡響應(yīng)速度和穩(wěn)定性，因此無法在保證響應(yīng)速度的同時(shí)滿足控制平穩(wěn)性要求，進(jìn)而導(dǎo)致無法在保證定位速度的同時(shí)滿足控制平穩(wěn)性要求；基于跟蹤微分器與狀態(tài)觀測器結(jié)合的船舶動力定位自抗擾控制方法，沒有考慮海洋環(huán)境高頻干擾影響，仍然會造成執(zhí)行機(jī)構(gòu)磨損嚴(yán)重，從而導(dǎo)致執(zhí)行機(jī)構(gòu)壽命短，同時(shí)這種沒有考慮海洋環(huán)境高頻干擾影響，從而導(dǎo)致定位速度緩慢的問題。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明目的是為了解決現(xiàn)有的船舶動力定位自抗擾控制方法還存在無法在保證定位速度的同時(shí)滿足控制平穩(wěn)性要求以及會導(dǎo)致執(zhí)行機(jī)構(gòu)壽命短的問題，而提出了一種基于自適應(yīng)擴(kuò)展卡爾曼濾波算法的動力定位自抗擾控制方法。

2、一種基于自適應(yīng)擴(kuò)展卡爾曼濾波算法的動力定位自抗擾控制方法，具體為：

3、步驟一、建立動力定位船低頻運(yùn)動模型；

4、步驟二、建立動力定位船高頻運(yùn)動模型，并將動力定位船高頻運(yùn)動模型轉(zhuǎn)換為狀態(tài)空間形式；

5、步驟三、利用步驟一獲得的動力定位船低頻運(yùn)動模型和步驟二獲得的狀態(tài)空間形式的動力定位船高頻運(yùn)動模型，獲得船舶運(yùn)動模型；

6、步驟四、設(shè)計(jì)非線性狀態(tài)誤差反饋控制律，利用非線性狀態(tài)誤差反饋控制律獲得螺旋槳施加的推力值；

7、步驟五、對步驟三獲得的船舶運(yùn)動模型進(jìn)行離散化，獲得離散化后的船舶運(yùn)動模型；

8、步驟六、基于自適應(yīng)擴(kuò)展卡爾曼濾波方法，利用步驟五獲得的離散化后的船舶運(yùn)動模型、步驟四獲得的螺旋槳施加的推力值設(shè)計(jì)動力定位自抗擾控制器。

9、進(jìn)一步地，所述步驟一中的建立動力定位船低頻運(yùn)動模型，具體為：

10、

11、

12、

13、

14、

15、

16、其中，xl是船舶低頻運(yùn)動的橫坐標(biāo)、yl是船舶低頻運(yùn)動的縱坐標(biāo)，ψl是船舶低頻運(yùn)動的角度，v是船舶低頻運(yùn)動的縱蕩線速度，橫蕩線速度，艏搖角速度組合的速度矢量，u是船舶低頻運(yùn)動的縱蕩線速度，v是船舶低頻運(yùn)動的橫蕩線速度，r是船舶低頻運(yùn)動艏搖角速度，τ是螺旋槳施加的推力向量，τ1是螺旋槳施加在縱蕩方向的推力，τ2是旋槳施加在橫蕩方向的推力，τ3是螺旋槳施加在艏搖方向的推力，b是環(huán)境干擾力向量，b1(t)是船舶低頻運(yùn)動縱蕩方向的環(huán)境干擾力，b2(t)是船舶低頻運(yùn)動橫蕩方向的環(huán)境干擾力，b3(t)是船舶低頻運(yùn)動艏搖方向的環(huán)境干擾力，t是時(shí)間，r(η)、r(ψ)是旋轉(zhuǎn)矩陣，m、c(v)、d(v)是中間矩陣，m是船舶質(zhì)量，xu,yv,yr,nv,nr、是水動力系數(shù)，xg是船舶重心位置，iz是船舶轉(zhuǎn)動慣量。

17、進(jìn)一步地，旋轉(zhuǎn)矩陣r(ψl)滿足下式：

18、

19、rt(ψl)s(r)r(ψl)＝r(ψl)s(r)rt(ψl)＝s(r)

20、

21、rt(ψl)r(ψl)＝i

22、||r(ψl)||＝1

23、其中，s(r)是中間矩陣，i是單位矩陣。

24、進(jìn)一步地，所述步驟二中的建立動力定位船高頻運(yùn)動模型，并將動力定位船高頻運(yùn)動模型轉(zhuǎn)換為狀態(tài)空間形式，具體為：

25、步驟二一、建立動力定位船高頻運(yùn)動模型，具體為：

26、

27、其中，s是拉普拉斯算子，kωi是i方向上的波浪強(qiáng)度的系數(shù)，ζi是i方向上的相對阻尼系數(shù)，ω0i是i方向上的波浪在pm波能譜中的主導(dǎo)頻率，i＝1,2,3分別表示縱蕩方向、橫蕩方向、艏搖方向；

28、步驟二二、將動力定位船高頻運(yùn)動模型轉(zhuǎn)換為狀態(tài)空間形式，具體為：

29、

30、

31、

32、ch＝[03×3i3×3]

33、

34、a22＝-diag{2ζ1ω01?2ζ2ω02?2ζ3ω03}

35、∑＝diag[kw1?kw2?kw3]

36、kwi＝2ξiω0iσi

37、其中，ηh＝[xh,yh,ψh]，ηh是船舶高頻運(yùn)動的船舶位置和艏向角組成的向量，xh是船舶高頻運(yùn)動過程中船舶橫坐標(biāo)，yh是船舶高頻運(yùn)動過程中的船舶縱坐標(biāo)，ψh是船舶高頻運(yùn)動過程中的船舶艏向角，ξh＝[xh,yh,ψh,ξx,ξy,ξψ]t，ξh是船舶高頻運(yùn)動的狀態(tài)向量，ξx是船舶高頻運(yùn)動過程中的縱蕩線速度，ξy是船舶高頻運(yùn)動過程中的橫蕩線速度，ξψ是船舶高頻運(yùn)動過程中的艏搖角速度，ωh＝[ωh1,ωh2,ωh3]t，ωh是零均值高斯白噪聲向量，ωh1,ωh2,ωh3是分別表示縱蕩方向、橫蕩方向、艏搖方向的零均值高斯白噪聲，αh、εh、ch為系數(shù)矩陣，a21、a22、∑是中間矩陣，kwi是中間變量，i3×3是3×3的單位矩陣，diag[]是對角矩陣，ξ1＝ξx，ξ2＝ξy，ξ3＝ξψ，σi是i方向上的波浪強(qiáng)度的常數(shù)，03×3是3×3的0矩陣。

38、進(jìn)一步地，所述步驟三中的利用步驟一獲得的動力定位船低頻運(yùn)動模型和步驟二獲得的狀態(tài)空間形式的動力定位船高頻運(yùn)動模型，獲得船舶運(yùn)動模型，具體為：

39、

40、其中，f(x)是狀態(tài)變量函數(shù)，b是推進(jìn)器配置控制矩陣，x是狀態(tài)變量，e是噪聲系數(shù)矩陣，ω、ω'是兩個零均值高斯白噪聲矩陣，h是觀測系數(shù)矩陣，y是系統(tǒng)輸出。

41、進(jìn)一步地，所述步驟四中的設(shè)計(jì)非線性狀態(tài)誤差反饋控制律，利用非線性狀態(tài)誤差反饋控制律獲得螺旋槳施加的推力值，具體為：

42、步驟四一、獲取系統(tǒng)狀態(tài)方程，并對系統(tǒng)狀態(tài)方程進(jìn)行離散化處理，獲得離散化后的系統(tǒng)狀態(tài)方程，具體為：

43、首先，獲取系統(tǒng)狀態(tài)方程：

44、

45、其中，x1是船舶位置和角度組成的向量，x2是船舶線速度和角速度組成的向量，x3是干擾加速度向量，是x1、x2、x3的導(dǎo)數(shù)，η'(t)是海洋環(huán)境干擾函數(shù)；

46、然后，對系統(tǒng)狀態(tài)方程進(jìn)行離散化處理，獲得離散化后的系統(tǒng)狀態(tài)方程：

47、

48、

49、其中，e1是船舶位置誤差和角度誤差組成的向量，z1是船舶位置和角度觀測值組成的向量，z2是船舶線速度和角速度觀測值組成的向量，z3是干擾加速度向量，是z1、z2、z3的導(dǎo)數(shù)，β01＞0，β02＞0，β03＞0，β01、β02、β03是觀測器的可調(diào)節(jié)增益，a1、a2是fal函數(shù)形狀參數(shù)，a取a1或a2，δ是增益區(qū)間系數(shù)；

50、步驟四二、利用離散化后的系統(tǒng)狀態(tài)方程獲取系統(tǒng)狀態(tài)反饋方程；

51、步驟四三、利用步驟四二獲得的系統(tǒng)狀態(tài)反饋方程獲得螺旋槳施加的推力向量的值。

52、進(jìn)一步地，所述步驟四二中的利用離散化后的系統(tǒng)狀態(tài)方程獲取系統(tǒng)狀態(tài)反饋方程，具體為：

53、令：

54、

55、其中，e2是線速度誤差和角速度誤差組成的向量，e3是干擾加速度誤差向量；

56、根據(jù)離散化后的系統(tǒng)狀態(tài)方程獲得誤差向量e1、e2、e3表達(dá)式如下：

57、

58、假設(shè)變量x1，x2為已知量，對任意v，以下方程有解：

59、f(x1,x2,τ)＝v

60、設(shè)τ＝g(x1,x2,v)則系統(tǒng)狀態(tài)反饋如下：

61、

62、上式最終改寫為：

63、

64、其中，v1是根據(jù)經(jīng)驗(yàn)預(yù)設(shè)的船舶位置和角度組成的預(yù)設(shè)向量，k1、k2是預(yù)設(shè)系數(shù)，是v1的導(dǎo)數(shù)。

65、進(jìn)一步地，所述步驟四三中的利用步驟四二獲得的系統(tǒng)狀態(tài)反饋方程獲得螺旋槳施加的推力向量的值，具體為：

66、首先，獲取螺旋槳施加的推力向量為：

67、

68、τ0＝k1fal(e1,α1,δ)+k2fal(e2,α2,δ)

69、其中，c為補(bǔ)償強(qiáng)度因子，z3(t)是t時(shí)刻的干擾加速度觀測值；

70、然后，設(shè)計(jì)固定時(shí)間擴(kuò)張狀態(tài)觀測器，具體為：

71、

72、w＝r(ψ)v

73、

74、

75、其中，是η的估計(jì)值，是的導(dǎo)數(shù)，w是輔助速度矢量，是w的估計(jì)值，χ是未知擾動集合，是χ的估計(jì)值，hn＜υ，μ1、μ2、μ3、ε1、ε2、ε3、υ是常數(shù)，hn是有界的常數(shù)，αi∈(0,1),βi＞1，是常數(shù)，l1，l2均為無窮小量，αi、βi是中間變量，i＝1,2,3；

76、固定時(shí)間擴(kuò)張狀態(tài)觀測器feso的增益取值范圍滿足以下hurwitz矩陣：

77、

78、

79、最后，基于固定時(shí)間擴(kuò)張狀態(tài)觀測器對到螺旋槳施加的推力向量離散化，獲得最終螺旋槳施加的推力向量，具體為：

80、

81、τ0(k)＝k1fal(e1,α1,δ)+k2fal(e2,α2,δ)

82、e2＝x2(k)-z2(k)

83、e1＝x1(k)-z1(k)

84、其中，k是離散化后的時(shí)間t，τ(k)是k時(shí)刻螺旋槳施加的推力，z3(k)是k時(shí)刻干擾加速度觀測值，τ0(k)是k時(shí)刻螺旋槳施加的推力理論值，x1(k)是k時(shí)刻船舶位置和角度組成的向量，z2(k)是k時(shí)刻船舶線速度和角速度觀測值組成的向量，z1(k)是k時(shí)刻船舶位置和角度觀測值組成的向量，x2(k)是k時(shí)刻船舶線速度和角速度組成的向量。

85、進(jìn)一步地，所述步驟五中的對步驟三獲得的船舶運(yùn)動模型進(jìn)行離散化，獲得離散化后的船舶運(yùn)動模型，具體為：

86、

87、δ＝a-1(φ-i)b

88、γ＝a-1(φ-i)e

89、r＝rt＞0

90、φ＝exp(ah)

91、其中，x(k)是k時(shí)刻系統(tǒng)狀態(tài)變量，x(k-1)是k-1時(shí)刻系統(tǒng)狀態(tài)變量，φ、δ、γ、h是系數(shù)矩陣，τ(k-1)是k-1時(shí)刻螺旋槳施加的推力，ω(k-1)是k-1時(shí)刻協(xié)方差矩陣為q的零均值高斯白噪聲，ω'(k-1)是k-1時(shí)刻協(xié)方差矩陣為r的零均值高斯白噪聲，y(k)是k時(shí)刻系統(tǒng)輸出，r、q分別是ω'(k-1)、ω(k-1)的協(xié)方差矩陣，a是狀態(tài)變量函數(shù)f(x)的系數(shù)矩陣，h是采樣時(shí)間，i是單位矩陣。

92、進(jìn)一步地，所述步驟六中的基于自適應(yīng)擴(kuò)展卡爾曼濾波方法，利用步驟五獲得的離散化后的船舶運(yùn)動模型、步驟四獲得的螺旋槳施加的推力值設(shè)計(jì)動力定位自抗擾控制器，具體為：

93、步驟六一、初始化系統(tǒng)狀態(tài)變量估計(jì)值的初值協(xié)方差估計(jì)值初值

94、步驟六二、利用離散化后的船舶運(yùn)動模型獲取k時(shí)刻系統(tǒng)狀態(tài)變量預(yù)測值：

95、

96、其中，是k-1時(shí)刻系統(tǒng)狀態(tài)變量估計(jì)值，τ(k-1)是k-1時(shí)刻螺旋槳施加的推力，是k時(shí)刻系統(tǒng)狀態(tài)變量預(yù)測值；

97、步驟六三、獲取誤差協(xié)方差預(yù)測值：

98、

99、其中，是k時(shí)刻誤差協(xié)方差預(yù)測值，是k-1時(shí)刻誤差協(xié)方差估計(jì)值；

100、步驟六四、引入遺忘因子ff，從而獲得更新調(diào)節(jié)參量d(k)：

101、

102、其中，d(k)是k時(shí)刻調(diào)節(jié)參量，ffk+1是ff的k+1次方，ff是遺忘因子；

103、步驟六五、利用步驟六二獲得的k時(shí)刻系統(tǒng)狀態(tài)變量預(yù)測值獲取k時(shí)刻系統(tǒng)輸出預(yù)測值：

104、

105、其中，是k時(shí)刻系統(tǒng)輸出的預(yù)測值；

106、步驟六六、利用步驟六五獲得的y(k)獲取系統(tǒng)輸出誤差：

107、

108、其中，y(k)是k時(shí)刻系統(tǒng)輸出實(shí)際值，是k時(shí)刻系統(tǒng)輸出預(yù)測值，z(k)是k時(shí)刻系統(tǒng)輸出誤差；

109、步驟六七、更新觀測噪聲協(xié)方差矩陣r(k)：

110、r(k)＝(1-d(k))r(k-1)

111、+d(k)([i-h(k)k(k-1)]w'(k)w't(k)[i-h(k)k(k-1)]t

112、+h(k)p(k-1)ht(k))

113、其中，r(k-1)是k-1時(shí)刻觀測噪聲協(xié)方差矩陣，h(k)是k時(shí)刻系數(shù)矩陣，w'(k)是k時(shí)刻高斯白噪聲，k(k-1)是k-1時(shí)刻的卡爾曼濾波增益系數(shù)，p(k-1)是k-1時(shí)刻濾波估計(jì)均方誤差矩陣；

114、步驟六八、利用步驟六三獲得的誤差協(xié)方差預(yù)測值計(jì)算kalman濾波增益系數(shù)k(k)：

115、

116、其中，k(k)是k時(shí)刻的卡爾曼濾波增益系數(shù)，r(k)是k時(shí)刻觀測噪聲協(xié)方差矩陣；

117、步驟六九、利用步驟六八獲得的k(k)和步驟六六獲得的z(k)獲取系統(tǒng)狀態(tài)變量估計(jì)值

118、

119、其中，是k時(shí)刻的系統(tǒng)狀態(tài)變量估計(jì)值；

120、步驟六十、利用步驟六八獲得的k(k)和步驟六七獲得的r(k)獲取誤差協(xié)方差估計(jì)值

121、

122、其中，是k時(shí)刻的誤差協(xié)方差估計(jì)值；

123、步驟六十一、利用誤差協(xié)方差預(yù)測值組成濾波估計(jì)均方誤差矩陣令k＝k+1，返回步驟六二。

124、本發(fā)明的有益效果為：

125、本發(fā)明基于自適應(yīng)擴(kuò)展卡爾曼濾波算法進(jìn)行動力定位自抗擾控制，更適用于具有非線性和不確定性的系統(tǒng)，本發(fā)明在擴(kuò)展卡爾曼濾波算法基礎(chǔ)上引入遺忘因子以及自適應(yīng)調(diào)整觀測噪聲協(xié)方差矩陣，對現(xiàn)有的動力定位自抗擾控制方法進(jìn)行了改進(jìn)，有效削弱初值對濾波器濾波效果的影響，提高了定位精度和定位速度。本發(fā)明參數(shù)調(diào)節(jié)簡單，能夠在提升響應(yīng)速度的同時(shí)，滿足控制平穩(wěn)性要求。本發(fā)明應(yīng)用自適應(yīng)擴(kuò)展卡爾曼濾波算法對海洋環(huán)境一階波浪力干擾進(jìn)行濾波處理，解決了動力定位船在海洋環(huán)境一階波浪力影響下船舶高頻往復(fù)運(yùn)動增加執(zhí)行機(jī)構(gòu)磨損問題，減少了執(zhí)行機(jī)構(gòu)損耗，提升了執(zhí)行機(jī)構(gòu)的壽命。