一種考慮局部避碰的uuv動目標滑模跟蹤控制方法
【專利摘要】本發(fā)明提供的是一種考慮局部避碰的UUV動目標滑模跟蹤控制方法。實時探測UUV、運動目標以及障礙物位置信息；獲取運動目標k時刻狀態(tài)估計；建立UUV和運動目標以及障礙物的相對運動模型；基于目標跟蹤半徑和障礙物規(guī)避安全半徑的大小，根據(jù)UUV與運動目標及障礙物的相對位置，在跟蹤策略和避碰策略之間自主切換；根據(jù)指令速度和航向與UUV航速及轉(zhuǎn)艏角速度測量反饋，得到跟蹤控制偏差，基于水平面非奇異終端滑?？刂破鹘馑愕玫絢時刻UUV推進器推力和方向舵轉(zhuǎn)艏力矩；循環(huán)執(zhí)行上述步驟，實現(xiàn)運動目標下一刻的跟蹤控制。本發(fā)明將復(fù)雜環(huán)境中的局部規(guī)劃策略與UUV動力學(xué)模型相結(jié)合，在UUV航行安全性的前提下保證對運動目標的跟蹤精度。
【專利說明】
-種考慮局部避碰的UUV動目標滑模跟蹤控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及的是一種水下無人航行器的目標跟蹤方法，具體地說是一種水下無人 航行器的動目標跟蹤方法。
【背景技術(shù)】
[0002] 隨著UUV使命任務(wù)和應(yīng)用領(lǐng)域的增加，對UUV目標跟蹤的能力要求也有大大提高， 要求其不僅能識別靜止的目標(例如援救定位盡快到達失事點、礦產(chǎn)開采、管道定位檢修）， 還可W跟蹤機動目標，通過預(yù)測目標狀態(tài)變化做出適應(yīng)性調(diào)整W免丟失跟蹤對象。
[0003] 目前，動目標跟蹤問題研究主要集中在航空航天等領(lǐng)域，主要指的是基于卡爾曼、 粒子濾波等目標狀態(tài)估計；比例導(dǎo)引法、尾追法、固定提前角法、平行接近法等動目標導(dǎo)引 方法;人工勢場、模糊避碰法等自主避障方法。針對UUV水下環(huán)境的研究還不夠成熟，對于動 態(tài)環(huán)境中執(zhí)行既定目標軌跡跟蹤過程中可能出現(xiàn)的礙航物考慮較少，即使有實時的運動規(guī) 劃，也很少結(jié)合UUV動力學(xué)模型及操縱性能通過運動控制加W實現(xiàn)。因此，將上述因素引入 UUV目標跟蹤控制中，真實反映其動態(tài)跟蹤避障能力，對于安全可靠執(zhí)行各種作業(yè)任務(wù)具有 重要意義。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的在于提供一種能夠?qū)崿F(xiàn)UUV對運動目標跟蹤、同時規(guī)避其航路上障 礙物的導(dǎo)引策略，穩(wěn)定快速、魯棒性強的考慮局部避碰的UUV動目標滑模跟蹤控制方法。
[0005] 本發(fā)明的目的是運樣實現(xiàn)的：
[0006] 步驟一 :UUV通過導(dǎo)航姿態(tài)傳感器、位置測量系統(tǒng)、前視聲響，實時探測UUV、運動目 標W及障礙物位置信息；
[0007] 步驟二:建立運動目標和障礙物運動學(xué)模型，基于不敏卡爾曼濾波獲取運動目標k 時刻狀態(tài)估計；
[000引步驟S:建立UUV和運動目標W及障礙物的相對運動模型，用于計算k時刻跟蹤距 離和視線角；
[0009] 步驟四：基于目標跟蹤半徑化和障礙物規(guī)避安全半徑化的大小，根據(jù)UUV與運動目 標及障礙物的相對位置，在跟蹤策略和避碰策略之間自主切換，改變UUV控制視線，規(guī)劃 UUVk時刻指令速度和航向；
[0010] 步驟五:根據(jù)步驟四中得到的指令速度和航向與UUV航速及轉(zhuǎn)臘角速度測量反饋， 得到跟蹤控制偏差，基于水平面非奇異終端滑?？刂破鹘馑愕玫絢時刻UUV推進器推力和方 向艙轉(zhuǎn)臘力矩；
[0011] 步驟六:循環(huán)執(zhí)行步驟一至步驟五，實現(xiàn)運動目標下一刻的跟蹤控制，直到任務(wù)結(jié) 束。
[0012] 本發(fā)明還可W包括；
[0013] 1、跟蹤策略和避碰策略之間自主切換的導(dǎo)引方式具體表示為：在全局跟蹤過程 中，如果出現(xiàn)P。i V , k《D 2 + ( R i。+ R V )，就激活避碰導(dǎo)引律式
，將視線切換到障礙物規(guī)避方面，一旦確認UUV運動 到安全區(qū)域就執(zhí)行目標跟蹤導(dǎo)引律；
，立即恢復(fù)跟 蹤過程，
[0014] 其中：Poiv,功k時亥Ijuuv與第i個障礙物之間的距離，UUV簡化為半徑Rv, Rio為第i個 障礙物的半徑，UvRef,k為k時刻UUV導(dǎo)引速度、rvRef,k為k時刻UUV導(dǎo)引轉(zhuǎn)臘角速度成為最大航 速、或為最小航速、IlVk為k時刻UUV的航向角、巧為UUV最大轉(zhuǎn)臘角速度、no為跟蹤速度增益、 m為跟蹤轉(zhuǎn)臘控制增益、m為避碰速度增益、m為避碰轉(zhuǎn)臘控制增益、Pvg,k為k時刻UUV與目 標之間的距離、4 Vg,k為k時刻UUV與目標的視線角即視線矢量與慣性坐標系E巧由之間的夾 角、(Kiv,功k時亥IjUUV與第i個障礙物之間的視線角、Di為目標跟蹤半徑、D2為障礙物規(guī)避安 全半徑、巧為飽和函數(shù)且取視線控制和飽和角速度的最小值、O (a)將轉(zhuǎn)角轉(zhuǎn)換到區(qū) 間[-JT，JT]。
[001引2、步驟五中所述UUV推進器推力和方向艙轉(zhuǎn)臘力矩具體表示為：
[0016]
[0017]
[001引其中：k時亥陽UV推進器推力Tu, k、k時刻方向艙轉(zhuǎn)臘力矩Tr, k、k時亥陽UV實際航速 Uv,k、k時刻UUV導(dǎo)引速度UvRef,k、k時刻UUV實際轉(zhuǎn)臘角速度rv,k、k時刻UUV導(dǎo)引轉(zhuǎn)臘角速度 rvRef,k、k時刻UUV橫向運動速度Vv,k;非奇異終端航速控制滑模面Sl和轉(zhuǎn)臘控制滑模面S2,滑 模面可調(diào)參數(shù)&>0、防>0前、心。2、(12為正奇數(shù)且1<~<2，><^( 2;航速跟蹤控制偏 也 Hi 差116,1< = 1^[?6:,廣1^,1{、臘向角速度巧制偏差'6,1{ =。6:,1{-1'乂，1{;"'||:=/"-.、,;.， 訊苗=W-Z..,，dll = Xu 巧 |u|u|u|，d22 = Yv 巧|v|v|v|，d33 = Zw+Z|w|w|w| 為動力學(xué)模 型參數(shù)；表示系統(tǒng)模型參數(shù)的估計值，且i巧,|乏M。，|乂 -車I含公"，i = 1，2，3，5，6， 表示模型參數(shù)的攝動量，Cisat(Si/4 1)、C2sat(S2/4 2)為滑?？刂破鞯牟贿B續(xù)切換項，Cl、 (1)1、C2、(62為改變抖振及攝動能力的可調(diào)參數(shù)。
[0019] 本發(fā)明導(dǎo)引策略的控制目標為:考慮UUV飽和約束條件下，二維動態(tài)環(huán)境中的目標 跟蹤和局部避障問題。①對于位置跟蹤，如果巧W,. (0 > %，則存在D1 > O，使得對于 V (,V-, (/。），.1',（/,,；)) G心有戶.,;（0 < ;②對于障礙物避碰問題，如果p〇iv( to) >〇2，則存在 化> 0，使對于V?！订栍锌凇?^ t) > (Rio+Rv)成立。
[0020] 其中，q=[x y 4]t表示慣性坐標系中位置和航向角，u=[u r]T表示航速和轉(zhuǎn)臘角 速度組成的運動學(xué)控制向量，用qv,k=[Xv,k yv,k 4v,k]T、Uv,k=[Uv,k rv,k]嗦示k時刻的UUV狀 態(tài)，qg,k=[xg,k yg,k 4g,k]T、Ug,k=[Ug,k rg,k]T表示k時刻的目標狀態(tài)，qi〇,k=[xi0,k yi0,k ih0,k]T、Ui〇,k=[Ui0,k ri0,k]T表示k時刻第i個障礙物狀態(tài)。將UUV簡化為半徑Rv，中屯、位置為 (XV,k，yv,k)的圓，類似得到半徑為Rg的目標和半徑為RiD的障礙物的簡化表示形式。Pvg, 時亥陽UV與目標之間的距離，d) Vg,功k時亥陽UV與目標的視線角（視線矢量與慣性坐標系EC 軸之間的夾角）;PDiv,k為k時亥IjUUV與第i個障礙物之間的距離，(Kiv,功k時亥IjUUV與第i個障 礙物之間的視線角;Di為目標跟蹤半徑，化為障礙物規(guī)避安全半徑。
[0021] 對于UUV速度和航向上的飽和約束，設(shè)最大航速為瑪、最大轉(zhuǎn)向變化率為吞，另外為 保證UUV潛伏在水下，其速度不能減速到零（會浮出水面），設(shè)最小航速為私，則有： 壞斗/一I ^兩，< ^，制定的導(dǎo)引策略具體如下：
[0022] (1)目標跟蹤導(dǎo)引律為：
[0023]
[0024] 其中，no為正常數(shù)速度增益，為保證跟蹤的平滑性，使UUV在落后目標較遠時W與 距離成正比的速度跟蹤;ni為轉(zhuǎn)臘控制增益，O (a)將轉(zhuǎn)角限制在區(qū)間[-31,31) 巧為飽 和函數(shù)，取視線控制和飽和角速度的最小值。
[0025] (2)避碰導(dǎo)引律為：
[0026]
[0027] 其中，0<n2《l轉(zhuǎn)速控制增益，ri3設(shè)計為正常數(shù)的轉(zhuǎn)臘控制增益。
[002引（3)視線切換策略：在全局跟蹤過程中，如若出現(xiàn)Poiv, k《化+ (Rio+Rv)的時刻，就激 活避碰導(dǎo)引律，將視線切換到障礙物規(guī)避方面，一旦確認UUV運動到安全區(qū)域就執(zhí)行目標跟 蹤導(dǎo)引律，立馬恢復(fù)跟蹤過程。
[0029] 本發(fā)明設(shè)計了如下的非奇異終端滑模速度控制器和轉(zhuǎn)臘控制器：
[0030] 川IV推徘柴推九巧曲I法責.
[0031]
[0032] UUV方向駝轉(zhuǎn)臘力矩控制律為：
[0033]
[0034] 其中，.巧.1 =掛-A;,，W:]二"i-K.，=W--Z',，.dii = Xu 巧 |u|u|u|，d22 = Yv 巧 |v|v|v ，(133 = 2^+2|本^|為動力學(xué)模型參數(shù)；"^"表示系統(tǒng)模型參數(shù)的估計值，且|聽-"\書馬， 佔.-4 |.《/?，i = I，2，3，5，6，表示模型參數(shù)的攝動量，Cisat (Si/ 4 I)、C2sat (S2/ 4 2)為滑模 控制器的不連續(xù)切換項，通過調(diào)整參數(shù)Cl, (62可增強控制器魯棒性并改善滑模抖振 現(xiàn)象。
[00對 UUV的航速跟蹤誤差為116,1< = 11巾1?6^1<-11巾,1<，轉(zhuǎn)臘角速度跟蹤誤差為。,1< =。6川寸乂,1<， 設(shè)計的非奇異Terminal滑模面分別為：
[0036]
[0037] 其中，執(zhí)>0，&>0，pi，qi，P2，q2為正奇數(shù)且1 <子<2 ' 1<戶<2。 H\ 啦
[0038] 本發(fā)明提供了一種能夠?qū)崿F(xiàn)UUV對運動目標跟蹤同時規(guī)避其航路上障礙物的導(dǎo)引 策略，并且結(jié)合UUV動力學(xué)模型和運動控制能力給出具體實現(xiàn)方法。本發(fā)明的有益效果在 于：
[0039] (1) WUUV與目標及障礙物之間的距離作為動態(tài)規(guī)劃準則，簡化了動態(tài)環(huán)境目標跟 蹤和局部避障問題的分析，考慮了UUV的運動控制約束制定的簡單有效切換策略，實現(xiàn)了 UUV安全性和跟蹤精度要求下的實時平穩(wěn)運動規(guī)劃。
[0040] (2)設(shè)計了非奇異終端滑?？刂破鳎軌蛳刂七^程中可能出現(xiàn)的系統(tǒng)奇異點， 有限時間內(nèi)實現(xiàn)對指令的穩(wěn)定控制，并且十分適用于水下非線性，存在環(huán)境干擾和模型參 數(shù)攝動的復(fù)雜工況。實現(xiàn)了考慮UUV動力學(xué)模型的有效跟蹤過程，穩(wěn)定快速、魯棒性強。
【附圖說明】
[0041] 圖1為本發(fā)明的總體框圖；
[0042] 圖2為平面中UUV目標跟蹤和被動避障雙目標控制問題示意圖；
[0043] 圖3為UUV目標跟蹤導(dǎo)引示意圖；
[0044] 圖4為UUV障礙物避碰導(dǎo)引示意圖；
[0045] 圖5為UUV跟蹤運動目標仿真案例軌跡；
[0046] 圖6a至圖6d為UUV跟蹤運動目標仿真案例的控制參數(shù)圖；
[0047] 圖7為UUV跟蹤運動目標同時規(guī)避動態(tài)障礙物仿真案例軌跡；
[004引圖8a至圖8d為UUV跟蹤運動目標同時規(guī)避動態(tài)障礙物仿真案例的控制參數(shù)圖；
[0049] 圖9是本發(fā)明的流程圖。
【具體實施方式】
[0050] 下面結(jié)合附圖對本發(fā)明做進一步說明：
[0051 ] UUV在執(zhí)行回收對接等任務(wù)時，需要尾隨跟蹤特定目標，對于水下復(fù)雜環(huán)境，在跟 蹤目標運動航路上不可避免會出現(xiàn)隨機障礙物(船只、浮游生物、水下垃圾等），針對此情況 本發(fā)明給出一種簡單有效的UUV動態(tài)環(huán)境跟蹤控制方法。
[0052] 圖1為本發(fā)明的總體工作框圖，主要分為UUV模型及探測系統(tǒng)、目標狀態(tài)估計、動態(tài) 規(guī)劃模塊和非奇異終端滑模控制器四個部分，目標和障礙物位置由UUV聲響探測系統(tǒng)獲取， UUV實時狀態(tài)信息由其組合導(dǎo)航系統(tǒng)測定，結(jié)合圖9描述本發(fā)明的具體步驟。
[0053] 步驟一 :UUV通過導(dǎo)航姿態(tài)傳感器、位置測量系統(tǒng)、前視聲響等，實時探測UUV、運動 目標W及障礙物位置信息(本發(fā)明的仿真圖由直接給出目標及障礙物運動軌跡得到）；
[0054] 步驟二:根據(jù)目標和障礙物運動規(guī)律，建立其運動學(xué)模型，基于軌跡點位置由不敏 卡爾曼(UK巧濾波算法獲取其k時刻狀態(tài)估計；
[005引設(shè)目標動態(tài)模型為Xk = FXk-i+GWk，狀態(tài)向量乂；二h，與，片，1'',.f，F(xiàn)為系統(tǒng)轉(zhuǎn)移矩陣， G為系統(tǒng)噪聲變換矩陣，系統(tǒng)狀態(tài)噪聲￥1^=[*、,1<，％,1^^為零均值高斯噪聲，協(xié)方差為91^。觀測 模型為Zk = h(Xk)+Vk，其中h為觀測函數(shù)，Vk為服從高斯分布的測量噪聲序列，協(xié)方差為Rk,且 存在
[0化6]
[0057] UKF算法的簡化流程為：
[0化引1.給定系統(tǒng)初值(初始狀態(tài)均值估計值芽。，初始狀態(tài)方差估計值Po)
[0化9]
[0060] 2.計算Sigma采樣點Xi及其權(quán)值A(chǔ)l，W最小的樣本集來近似系統(tǒng)狀態(tài)的分布函數(shù)
[0061]
[0062] 其中n為狀態(tài)向量的維度，K為尺度參數(shù)，本設(shè)計中取2。
[0063] 3.預(yù)測/時間更新，對Sigma采樣點進行非線性變換和加權(quán)處理，得到狀態(tài)向量均 值、狀態(tài)向量方差和輸出向量的一步預(yù)測值、Giw、契IM
[0064]
[00 化]
[0066]
[0067]
[006引
[0069] 使用變換后的狀態(tài)變量進行濾波估計，W減小估計誤差，同時由于該算法采用了 非線性的動態(tài)方程或量測方程，避免了線性化誤差。
[0070] 4.校正/測量更新
[0071]
[0072]
[007；3] (4)
[0074]
[0075]
[0076] 其中，Yk為k時刻觀測值，iV、Pk分別為k時刻輸出向量和狀態(tài)向量方差，巧,,,為其兩 者協(xié)方差，Mk為UKF校正增益，即為k時刻狀態(tài)的濾波估計值。
[0077] 步驟根據(jù)步驟二中濾波后的狀態(tài)信息，基于UUV和運動目標W及障礙物的相對 運動模型，計算k時刻的跟蹤距離和視線角，如圖2所示的平面中UUV目標跟蹤和被動避障雙 目標控制問題；
[0078] 在制定導(dǎo)引策略時，認為UUV橫向速度是個微小的禪合量，對運動軌跡影響較小， 設(shè)計時將運動學(xué)模型簡化為如式(5)的形式，并且假定目標和障礙物滿足相同的運動規(guī)律：
[0079]
[0080] 其中q=[x y 4]t表示慣性坐標系中位置和航向角，u=[u r]T表示航速和轉(zhuǎn)臘角 速度組成的運動學(xué)控制向量。k時刻，對于UUVW向量qv,k=[xv,k yv,k ilVk]T、Uv,k=[Uv,k Tv,k]來表不，目柄 Wqg,k=[Xg,k yg,k 4g,k]、Ug,k=[Ug,k rg,k]來表不，束i個P早礙物 Wqi〇,k = [xi0,k yi0,k ih0,k]T、Ui〇,k=[Ui0,k ri0,k]T來表示;在示意圖中UUV簡化為半徑Rv，中屯、位置 為(XV,k，yv,k)的圓，類似得到半徑為Rg的目標和半徑為RiD的障礙物的簡化表示形式。
[0081 ]帶入UUV和目標的相對運動模型：
[0082]
[0083]
[0084] 其中，Pvg,功k時刻UUV與目標之間的距離，d) Vg,功k時刻UUV與目標的視線角(視線 矢量與慣性坐標系EC軸之間的夾角）。
[0085] 式(6)取微分形式，聯(lián)立式(5)推導(dǎo)得到UUV與目標的跟蹤誤差形式：
[0089] 其中，Pniv,k為k時刻UUV與第i個障礙物之間的距離，d) Div,k為k時亥陽UV與第i個障 礙物之間的視線角。
[0086]
[0087]
[008引
[0090] 步驟四：基于步驟S中計算得到的k時刻UUV與目標及障礙物的相對位置，結(jié)合目 標跟蹤半徑化和障礙物規(guī)避安全半徑化的要求，制定全局跟蹤和局部避碰自主切換策略，改 變UUV控制視線，規(guī)劃其k時刻的指令速度UvRef,k和指令航向rvRef,k;
[0091] (1)目標跟蹤導(dǎo)引律:設(shè)計在最大轉(zhuǎn)臘角速度的限制下，控制UUV航向盡快向跟蹤 視線方向轉(zhuǎn)動，并且當UUV在目標接近圓內(nèi)時W目標速度保持監(jiān)視，當目標機動逃到跟蹤圈 W外時UUV采用最大航速追蹤，如圖3所示。盡量選用恒定線速度的方式有利于減少控制量， W轉(zhuǎn)臘控制來增加航速控制的靈活性，具體可表示為：
[0092]
[009引其中，UUV有最大航速巧、最大轉(zhuǎn)向變化率巧的飽和約束，并為保證UUV潛伏在水下， 設(shè)最小航速為餐，即成^ ^吊，《巧;no為正常數(shù)速度增益，為保證跟蹤的平滑性，使 UUV在落后目標較遠時W與距離成正比的速度跟蹤;M聲/，巧為飽和函數(shù)，取視線控制和飽 和角速度的最小值，函數(shù)表示為：
[0094] .sw(i/，巧二巧ti("(〇) ? min(|"(〇|，巧（10)
[0095] m為轉(zhuǎn)臘控制增益，O (a)將轉(zhuǎn)角限制在區(qū)間[-n，n)，定義為：
[0096]
[0097] (2)避碰導(dǎo)引律：當目標進入半徑為化的障礙物危險圓W內(nèi)時，設(shè)計在最大轉(zhuǎn)臘角 速度的限制下，控制UUVW最大線速度盡快轉(zhuǎn)向遠離障礙物的視線方向，如圖4所示的避障 導(dǎo)引策略，函數(shù)表示為：
[009引
[0099] 其中，0<n2《l轉(zhuǎn)速控制增益，ri3設(shè)計為正常數(shù)的轉(zhuǎn)臘控制增益。
[0100] (3)視線切換策略:基于W上兩種情況的導(dǎo)引，可W證明存在(n〇，m，Di)使得UUV對 目標的跟蹤誤差一致收斂于Di,存在(n2，n3，D2)使得在初始時刻的條 件下，系統(tǒng)能夠保證成功避開障礙物。得到動態(tài)環(huán)境中目標跟蹤同時被動避障的策略是:在 全局跟蹤過程中，如若出現(xiàn)P。lv,k《D2+(Rl。+Rv)的時刻，就激活避碰導(dǎo)引律，將視線切換到障 礙物規(guī)避，一旦確認UUV運動到安全區(qū)域就立馬恢復(fù)跟蹤過程。
[0101] 步驟五:制定跟蹤策略后還需要結(jié)合動力機構(gòu)加W控制，因此根據(jù)步驟四中的導(dǎo) 引指令與UUV航速及轉(zhuǎn)臘角速度測量反饋，得到跟蹤控制偏差，設(shè)計對于參數(shù)攝動及環(huán)境干 擾具有很強魯棒性的非奇異終端滑?？刂破鳎馑愕玫絢時刻UUV推進器推力Tu,k和方向艙 轉(zhuǎn)臘力矩Tr,k;
[0102] 對于水平面UUV動力學(xué)模型具有如下簡化形式：
[0103]
[0104]
[0105]
[0106] 將步驟四中得到的導(dǎo)引律UvRef,k和rvRef,k作為k時刻運動控制期望指令，與UUV實際
[010引其中，可調(diào)參數(shù)執(zhí)>0，&>0，口1，叫口2,92為正奇數(shù)且1<"^<2，1<^^< 2。H\ 恥 航速航向相比較，得到跟蹤控制偏差Ue,k = UvRef,k-Uv,k，re,k = rref,k-：rv,k，基于非奇異終端滑 模原理分面I巧'ff 掉H舒每曲K曾措:而.
[0107]
[0109]推導(dǎo)得到水平面UUV推力和轉(zhuǎn)臘力矩控制律：
[0110；
[0111] 其中，",M 乂" ' =W-Z.''，dll = Xu 巧 |u|u|u|，d22 = Yv 巧 |v|v|v ，(133 = 2^+2|本^|為動力學(xué)模型參數(shù)；"~"表示系統(tǒng)模型參數(shù)的估計值，且|"!,,-?>\|心梯"， I 電-.:4 1.^? , i = 1，2，3，5，6，表示模型參數(shù)的攝動量，Cisat (Si/ 4 1)、C2sat (S2/ 4 2)為滑模 控制器的不連續(xù)切換項。
[0112] 構(gòu)造 Lyapunov函I
勻正定，分別求導(dǎo)可^證明乂負 定，由此可知UUV跟蹤控制偏差能在有限時間向零狀態(tài)穩(wěn)定，即實際航速和實際轉(zhuǎn)臘角速度 能在有限時間內(nèi)跟蹤導(dǎo)引指令。
[0113] 步驟六:k+1時刻，跳轉(zhuǎn)至步驟一，執(zhí)行步驟一~步驟五，得到實時跟蹤控制Tu, k+i和 Tr, W ;隨著采樣進行，循環(huán)上述過程直到收到任務(wù)結(jié)束指令。
[0114] 給出本發(fā)明的兩個實例仿真，軌跡及運動控制參數(shù)圖分別見附圖。圖5、圖6a至圖 6d為機動目標跟蹤過程，整個過程相對平穩(wěn)；圖7、圖8a至圖8d展示的是跟蹤環(huán)境未知，出現(xiàn) 運動障礙物在航線上干擾的情況，對突變的運動指令仍然能快速穩(wěn)定控制。
[0115] 設(shè)隨機運動障礙物軌跡方程為
[0116]
[0117] UUV半徑Rv = 3m，運動障礙物半徑Ro = 5m，目標接近圓半徑化=IOm，障礙物風險區(qū) 半徑化= 25m，數(shù)據(jù)采集間隔To = O.5s?？蒞看出滑?？刂破骶葮O高，對控制指令響應(yīng)快誤 差小，UUV實際航跡基本能夠與規(guī)劃軌跡重合。在圖7案例中，本仿真方法一方面能夠保證對 動目標的跟蹤精度，另一方面化v(t)《D2+(R〇+Rv) =33m時激活避障控制直至Pov(t) >D2+(R。 +Rv)，短時避障期間Pov(min) = 26.8024m>R〇+Rv = 8m始終保證了UUV的航行安全性。
【主權(quán)項】
1. 一種考慮局部避碰的UUV動目標滑模跟蹤控制方法，其特征是： 步驟一 :UUV通過導(dǎo)航姿態(tài)傳感器、位置測量系統(tǒng)、前視聲吶，實時探測UUV、運動目標以 及障礙物位置信息； 步驟二:建立運動目標和障礙物運動學(xué)模型，基于不敏卡爾曼濾波獲取運動目標k時刻 狀態(tài)估計； 步驟三:建立UUV和運動目標以及障礙物的相對運動模型，用于計算k時刻跟蹤距離和 視線角； 步驟四：基于目標跟蹤半徑Di和障礙物規(guī)避安全半徑02的大小，根據(jù)UUV與運動目標及 障礙物的相對位置，在跟蹤策略和避碰策略之間自主切換，改變UUV控制視線，規(guī)劃UUVk時 刻指令速度和航向； 步驟五:根據(jù)步驟四中得到的指令速度和航向與UUV航速及轉(zhuǎn)艏角速度測量反饋，得到 跟蹤控制偏差，基于水平面非奇異終端滑?？刂破鹘馑愕玫絢時刻UUV推進器推力和方向舵 轉(zhuǎn)艏力矩； 步驟六:循環(huán)執(zhí)行步驟一至步驟五，實現(xiàn)運動目標下一刻的跟蹤控制，直到任務(wù)結(jié)束。2. 根據(jù)權(quán)利要求1所述的一種考慮局部避碰的UUV動目標滑模跟蹤控制方法，其特征是 跟蹤策略和避碰策略之間自主切換的導(dǎo)引方式具體表示為:在全局跟蹤過程中，如果出現(xiàn) Poi^kSDs+Uio+Rv)，就激活避碰導(dǎo)引律式，將視線切 換到障礙物規(guī)避方面，一旦確認UUV運動到安全區(qū)域就執(zhí)行目標跟蹤導(dǎo)引律式，立即恢復(fù)跟蹤過程， 其中：P〇iv,k為k時亥ljuuv與第i個障礙物之間的距離，Ri。為第i個障礙物的半徑，UUV簡化 為半徑Rv，uvRrf,k為k時刻UUV導(dǎo)引速度、rvRrf,k為k時刻UUV導(dǎo)引轉(zhuǎn)艏角速度、巧為最大航速、免 為最小航速、ikk為k時刻UUV的航向角、ξ為UUV最大轉(zhuǎn)艏角速度、no為跟蹤速度增益、m為跟 蹤轉(zhuǎn)艏控制增益、Π 2為避碰速度增益、Π 3為避碰轉(zhuǎn)艏控制增益、Pvg,k為k時刻UUV與目標之間 的距離、<Kg,k為k時刻UUV與目標的視線角即視線矢量與慣性坐標系Εξ軸之間的夾角、 (K lv,k為k時刻UUV與第i個障礙物之間的視線角、Di為目標跟蹤半徑、D2為障礙物規(guī)避安全 半徑、為飽和函數(shù)且取視線控制和飽和角速度的最小值、Φ(α)將轉(zhuǎn)角轉(zhuǎn)換到區(qū)間 [-3T,Jl] 〇3. 根據(jù)權(quán)利要求1或2所述的一種考慮局部避碰的UUV動目標滑模跟蹤控制方法，其特 征是步驟五中所述UUV推進器推力和方向舵轉(zhuǎn)艏力矩具體表示為：其中：k時亥ljuuv推進器推力D、k時刻方向舵轉(zhuǎn)艏力矩h,k、k時刻UUV實際航速Uv,k、k時 刻UUV導(dǎo)引速度uvRef,k、k時刻UUV實際轉(zhuǎn)艏角速度rv,k、k時亥ljuuv導(dǎo)引轉(zhuǎn)艏角速度r vRef,k、k時 刻UUV橫向運動速度Vv,k;非奇異終端航速控制滑模面 S1和轉(zhuǎn)艏控制滑模面s2,滑模面可調(diào)參 數(shù)如>0、此>〇414142、92為正奇數(shù)且；航速跟蹤控制偏差Ue,k = 1^射,1<-1^,1<、艏向角速度控制偏差1'(3,1< = 1^,1<-1\,1<;講1.1=..樹-義1;:，樹22.=.冊-（，=Μ-Ζ^ , dll = Xu+X|u|u |u I，d22 = Yv+Y|v|v| V I，d33 = Zw+Z|w|w| w| 為動力學(xué)模型參數(shù)；"~"表示系統(tǒng)模型參 數(shù)的估計值，且|m,7 -?;? ，|<4 -元，i = l，2,3,5,6,表示模型參數(shù)的攝動量，Cisat (Sl/<i>l)、C2Sat(S2/<i)2)為滑模控制器的不連續(xù)切換項，Cl、<i>l、C2、Φ2為改變抖振及攝動能 力的可調(diào)參數(shù)。
【文檔編號】G05D1/02GK105955268SQ201610312434
【公開日】2016年9月21日
【申請日】2016年5月12日
【發(fā)明人】嚴浙平, 陳燁, 侯茹萍, 周佳加, 張偉, 羅朋飛
【申請人】哈爾濱工程大學(xué)