本發(fā)明屬于水下機(jī)器人控制領(lǐng)域，尤其涉及一種基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法。

背景技術(shù)：
占地球表面積75％的海洋，是一個(gè)富饒而遠(yuǎn)未得到開發(fā)的寶庫。人類要維持生存繁衍和發(fā)展，充分利用地球僅有的這塊最后待開發(fā)的疆土，將是無可回避的選擇。水下機(jī)器人由于其行動(dòng)靈活，能夠在水中長時(shí)間工作而日益成為人類開發(fā)利用海洋資源的重要工具，同時(shí)軍用智能水下機(jī)器人在軍事上也有用武之地，因此水下機(jī)器人的研制和應(yīng)用具有重要的戰(zhàn)略意義。水下機(jī)器人的推力分配方法對(duì)其實(shí)現(xiàn)準(zhǔn)確的定位、航行、作業(yè)、容錯(cuò)控制等具有重要作用。水下機(jī)器人要在水中實(shí)現(xiàn)穩(wěn)定地航行和作業(yè)，既要求保持水平位置，又要抵抗海流影響；既要保持下潛深度，又要在作業(yè)中保持姿態(tài)，使其不會(huì)造成過大的橫傾和縱傾；既要優(yōu)化使用其推力節(jié)約能量，還要在必要的時(shí)候?qū)崿F(xiàn)容錯(cuò)控制。所以必須通過水下機(jī)器人的推力優(yōu)化分配才能使水下機(jī)器人更加廣泛地應(yīng)用于水下工程、海洋石油資源開發(fā)、海洋礦物資源調(diào)查、海洋生物資源調(diào)查、深海打撈、核潛艇救生，船體檢測(cè)等方面。申請(qǐng)?zhí)枮镃N201410583348的中國專利文件(公開日：2015年2月4號(hào))中公開的“基于CAM矩陣的水下機(jī)器人矢量推力分配方法”提供了一種針對(duì)多自由度矢量推進(jìn)水下機(jī)器人推力分配的方法?；贑AM矩陣的水下機(jī)器人矢量推力分配方法雖然與本專利屬于同一技術(shù)領(lǐng)域，但它只是將水下機(jī)器人每個(gè)多自由度矢量推進(jìn)器分配貢獻(xiàn)系數(shù)，通過將各自由度需求指令組成的向量與每個(gè)推進(jìn)器自由度對(duì)應(yīng)的貢獻(xiàn)系數(shù)向量做內(nèi)積,得到推進(jìn)器在該方向上的控制指令。

技術(shù)實(shí)現(xiàn)要素：
本發(fā)明的目的是提供一種控制精度高的，基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法。一種基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法，包括以下步驟，步驟一，針對(duì)水下機(jī)器人的推進(jìn)器布置情況建立推力分配模型，對(duì)矢量推進(jìn)器在每個(gè)自由度方向推力進(jìn)行2范數(shù)和無窮大泛數(shù)雙重判據(jù)的推力優(yōu)化分配；步驟二：使用對(duì)偶原理將推力優(yōu)化分配的原問題轉(zhuǎn)化為線性等式的對(duì)偶問題，利用神經(jīng)網(wǎng)絡(luò)方法求解該線性等式得到推力分配的優(yōu)化解；步驟三：根據(jù)每個(gè)推進(jìn)器的控制電壓—推力曲線，將優(yōu)化解得到的每個(gè)推進(jìn)器推力進(jìn)行多次艾特金插值，得到推進(jìn)器的輸出電壓，實(shí)現(xiàn)水下機(jī)器人多個(gè)自由度的運(yùn)動(dòng)控制。本發(fā)明一種基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法，還可以包括：1、步驟一中雙重判據(jù)的推力優(yōu)化分配的求取方法為：首先根據(jù)水下機(jī)器人的推進(jìn)器的布置形式建立推力分配模型，建立和水下機(jī)器人慣性主軸重合的坐標(biāo)系OXYZ分別指向機(jī)器人移動(dòng)的縱向、橫向和垂向；在水下機(jī)器人坐標(biāo)系OXYZ下推進(jìn)器相對(duì)于質(zhì)心的矢量和方向?yàn)閇r1r2…rn]和[e1e2...en]，其中ri＝[xiyizi]，ei＝[cosβisinβisinαi]，第i個(gè)推進(jìn)器對(duì)機(jī)器人產(chǎn)生的推力是iF，力矩是iM，當(dāng)自動(dòng)控制中的推力分配模塊收到水下機(jī)器人的控制指令后，根據(jù)控制指令和推進(jìn)器布置形式得到水下機(jī)器人所需的控制力矩陣為：水下機(jī)器人推力的2范數(shù)形式為其F＝[1F2F...nF]，推力的無窮大范數(shù)形式為||F||∞＝max{|1F||2F|…|nF|}，得到雙重判據(jù)的推力優(yōu)化分配問題：stτ＝RF式中a∈(0,1]是權(quán)值系數(shù)。2、將推力優(yōu)化分配的原問題轉(zhuǎn)化為線性等式為：KΩ(g-(Hg+P))-g＝0其中，KΩ(·):R3n+n+1→Ω為從空間R3n+n+1到集合Ω的一線性投影操作數(shù)，其中KΩ(g)中第i個(gè)元素定義為：g＝[Sd2d1]T,S＝[1F2F...4F||F||∞]，F(xiàn)l＝[1Fl2Fl...nFl]T，和Fu＝[1Fu2Fu...nFu]T,分別表示推進(jìn)器推力的下飽和界和上飽和界，τu和τl分別為τ的上界和下界，1n＝[11...1]T；建立將推力優(yōu)化分配的原問題轉(zhuǎn)化為線性等式的對(duì)偶問題的神經(jīng)網(wǎng)絡(luò)優(yōu)化求解器：其中γ＞0是神經(jīng)網(wǎng)絡(luò)的比例系數(shù)，g為最終求得的優(yōu)化解；cij是C＝I+HT的第i行第j列元素,tik是S＝I-H的第i行第j列元素。有益效果：本發(fā)明是通過對(duì)矢量推進(jìn)器在每個(gè)自由度方向推力進(jìn)行2范數(shù)和無窮大泛數(shù)雙重判據(jù)的推力優(yōu)化分配，可最大限度地利用推進(jìn)器同時(shí)完成機(jī)器人的多自由度運(yùn)動(dòng)，對(duì)水下機(jī)器人的精確控制、容錯(cuò)控制、抗擾控制、作業(yè)過程中的姿態(tài)穩(wěn)定控制等有重要作用。本發(fā)明目的是提供一種基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法，該方法能夠有效地將水下機(jī)器人六自由度的控制指令優(yōu)化分配到各個(gè)推進(jìn)器上，可用于開架式水下機(jī)器人的精確控制、容錯(cuò)控制和水下機(jī)器人作業(yè)過程中的姿態(tài)穩(wěn)定控制等工作。該發(fā)明具有使用方便、靈活，適應(yīng)性強(qiáng)并且具有一定通用性等優(yōu)點(diǎn)。附圖說明圖1是本發(fā)明的實(shí)現(xiàn)方法流程圖；圖2是對(duì)偶問題(2)的優(yōu)化求解器式(4)的框圖；圖3是式(5)神經(jīng)網(wǎng)絡(luò)優(yōu)化求解器框圖；圖4是應(yīng)用本發(fā)明進(jìn)行容錯(cuò)控制的SY-2水下機(jī)器人及其推進(jìn)器布置圖；圖4(a)是SY-2水下機(jī)器人實(shí)物圖，圖4(b)是SY-2水下機(jī)器人推進(jìn)器布置示意圖；圖5是敞水試驗(yàn)得到的SY-2水下機(jī)器人推進(jìn)器的控制電壓—推力曲線；圖5(a)是SY-2水下機(jī)器人正車推力曲線，圖5(b)是SY-2水下機(jī)器人倒車推力曲線；圖6是應(yīng)用本發(fā)明對(duì)SY-2水下機(jī)器人進(jìn)行容錯(cuò)控制的控制曲線；圖7是某作業(yè)型無人水下航行器-機(jī)械手系統(tǒng)及其推進(jìn)器布置圖；圖7(a)是某作業(yè)型無人水下航行器，圖7(b)是航行器機(jī)械手系統(tǒng)，圖7(c)是航行器推進(jìn)器布置圖；圖8是作業(yè)過程中使用本發(fā)明進(jìn)行優(yōu)化控制得到的機(jī)器人姿態(tài)曲線；圖8(a)是機(jī)械手關(guān)節(jié)角曲線，圖8(b)是規(guī)劃位姿曲線；圖9是仿真得到的作業(yè)時(shí)水下機(jī)器人受到的機(jī)械手?jǐn)_動(dòng)力，圖9(a)是機(jī)械手?jǐn)_動(dòng)力曲線，圖9(b)是機(jī)械手?jǐn)_動(dòng)力矩曲線。具體實(shí)施方式下面將結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步詳細(xì)說明。發(fā)明目的實(shí)現(xiàn)步驟為：1)針對(duì)水下機(jī)器人的推進(jìn)器布置情況建立推力分配模型，對(duì)矢量推進(jìn)器在每個(gè)自由度方向推力進(jìn)行2范數(shù)和無窮大泛數(shù)雙重判據(jù)的推力優(yōu)化分配；2)使用對(duì)偶原理將推力優(yōu)化分配的原問題轉(zhuǎn)化為線性等式的對(duì)偶問題，利用神經(jīng)網(wǎng)絡(luò)方法求解該線性等式得到推力分配的優(yōu)化解；3)根據(jù)每個(gè)推進(jìn)器的控制電壓—推力曲線，將優(yōu)化解得到的每個(gè)推進(jìn)器推力進(jìn)行多次艾特金插值，得到推進(jìn)器的輸出電壓，從而實(shí)現(xiàn)水下機(jī)器人多個(gè)自由度的運(yùn)動(dòng)控制。發(fā)明的詳細(xì)實(shí)現(xiàn)方法為：a、首先根據(jù)水下機(jī)器人的推進(jìn)器的布置形式建立推力分配模型。建立和水下機(jī)器人慣性主軸重合的坐標(biāo)系OXYZ分別指向機(jī)器人移動(dòng)的縱向、橫向和垂向。在水下機(jī)器人坐標(biāo)系OXYZ下推進(jìn)器相對(duì)于質(zhì)心的矢量和方向?yàn)閇r1r2...rn]和[e1e2...en]，其中ei＝[cosβisinβisinαi]，ri＝[xiyizi]，設(shè)第i個(gè)推進(jìn)器對(duì)機(jī)器人產(chǎn)生的推力是iF，力矩是iM，則當(dāng)自動(dòng)控制中的推力分配模塊收到水下機(jī)器人的控制指令后，根據(jù)控制指令和推進(jìn)器布置形式得到水下機(jī)器人所需的控制力矩陣為：設(shè)水下機(jī)器人推力的2范數(shù)形式為其F＝[1F2F...nF]，推力的無窮大范數(shù)形式為||F||∞＝max{|1F||2F|…|nF|}，推力分配問題按下式轉(zhuǎn)化為下面的優(yōu)化問題：式中a∈(0,1]是權(quán)值系數(shù)b、設(shè)S＝[1F2F…4F||F||∞]，其中Fl＝[1Fl2Fl…nFl]T，F(xiàn)u＝[1Fu2Fu…nFu]T,分別表示推進(jìn)器推力的下飽和界和上飽和界，τu和τl分別為τ的上界和下界，1n＝[11…1]T,設(shè)g＝[Sd2d1]T,設(shè)KΩ(·):R3n+n+1→Ω為從空間R3n+n+1到集合Ω的一線性投影操作數(shù)，其中KΩ(g)中第i個(gè)元素定義為：則優(yōu)化問題(2)等價(jià)于線性等式：KΩ(g-(Hg+P))-g＝0(4)c、根據(jù)式(4)得到一個(gè)原問題-對(duì)偶問題的神經(jīng)網(wǎng)絡(luò)優(yōu)化求解器：式中γ＞0是神經(jīng)網(wǎng)絡(luò)的比例系數(shù)，g即為最終求得的優(yōu)化解。設(shè)cij是C＝I+HT的第i行第j列元素,tik是S＝I-H的第i行第j列元素，則式(5)的計(jì)算機(jī)實(shí)現(xiàn)形式為：d、將優(yōu)化解得到的每個(gè)推進(jìn)器推力根據(jù)所對(duì)應(yīng)的該推進(jìn)器敞水試驗(yàn)曲線進(jìn)行多次艾特金插值這樣就可以有效地將水下機(jī)器人的六自由度運(yùn)動(dòng)控制指令優(yōu)化分配到各個(gè)推進(jìn)器上，得到推進(jìn)器的輸出電壓。本發(fā)明提供的是一種基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法，該方法能夠有效地將水下機(jī)器人六自由度的控制指令優(yōu)化分配到各個(gè)推進(jìn)器上。首先針對(duì)水下機(jī)器人的推進(jìn)器布置情況建立推力分配模型，其次對(duì)矢量推進(jìn)器在每個(gè)自由度方向推力進(jìn)行2范數(shù)和無窮大泛數(shù)雙重判據(jù)的推力優(yōu)化分配；然后使用對(duì)偶原理將推力優(yōu)化分配的原問題轉(zhuǎn)化為線性等式的對(duì)偶問題，利用神經(jīng)網(wǎng)絡(luò)方法求解該線性等式得到推力分配的優(yōu)化解；最后根據(jù)每個(gè)推進(jìn)器的控制電壓—推力曲線，將優(yōu)化解得到的每個(gè)推進(jìn)器推力進(jìn)行多次艾特金插值，得到推進(jìn)器的輸出電壓，從而實(shí)現(xiàn)水下機(jī)器人多個(gè)自由度的運(yùn)動(dòng)控制。該方法可最大限度地利用推進(jìn)器同時(shí)完成機(jī)器人的多自由度運(yùn)動(dòng)，對(duì)水下機(jī)器人的精確控制、容錯(cuò)控制、抗擾控制、作業(yè)過程中的姿態(tài)穩(wěn)定控制等方面有重要作用。本發(fā)明提供的一種基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法，通過對(duì)矢量推進(jìn)器在每個(gè)自由度方向的2范數(shù)和無窮大泛數(shù)的雙重判據(jù)優(yōu)化分配，可最大限度地利用推進(jìn)器同時(shí)完成機(jī)器人的多自由度運(yùn)動(dòng)。應(yīng)用本發(fā)明所提供的方法實(shí)現(xiàn)了SY-2開架水下機(jī)器人的容錯(cuò)控制實(shí)驗(yàn)，某水下機(jī)器人作業(yè)過程中的手艇協(xié)調(diào)控制仿真。實(shí)施步驟如圖1所示：首先根據(jù)水下機(jī)器人推進(jìn)器布置形式建立推力分配模型，再依據(jù)控制指令和推進(jìn)器布置形式得到期望控制力矩陣，將其表示成推力的2范數(shù)形式和無窮大范數(shù)形式，則推力分配問題可轉(zhuǎn)化成基于雙重判據(jù)的推力優(yōu)化問題。使用對(duì)偶原理將推力優(yōu)化分配的原問題轉(zhuǎn)化線性等式(7)，利用神經(jīng)網(wǎng)絡(luò)方法求解線性等式得到推力分配的優(yōu)化解(求解框圖如圖2和圖3所示)。然后根據(jù)該推進(jìn)器的控制電壓—推力曲線，將優(yōu)化解得到的每個(gè)推進(jìn)器推力進(jìn)行多次艾特金插值，得到推進(jìn)器的輸出電壓，從而實(shí)現(xiàn)水下機(jī)器人多個(gè)自由度的運(yùn)動(dòng)控制。結(jié)合圖2，設(shè)第i個(gè)推進(jìn)器對(duì)機(jī)器人產(chǎn)生的推力是iF，F(xiàn)＝[1F2F…nF]，其中Fl＝[1Fl2Fl...nFl]T，F(xiàn)u＝[1Fu2Fu...nFu]T,分別表示推進(jìn)器推力的下飽和界和上飽和界，τu和τl分別為τ的上界和下界，1n＝[11...1]T,S＝[1F2F…4F||F||∞]，g＝[Sd2d1]T,設(shè)KΩ(·):R3n+n+1→Ω為從空間R3n+n+1到集合Ω的一線性投影操作數(shù)，其中KΩ(g)中第i個(gè)元素定義為：則推力優(yōu)化分配的原問題(2)轉(zhuǎn)化為線性等式(8)：圖3描述了本發(fā)明求解線性等式(推力優(yōu)化分配的對(duì)偶問題)神經(jīng)網(wǎng)絡(luò)求解結(jié)構(gòu)框圖，網(wǎng)絡(luò)包括輸入層、隸屬度層、規(guī)則層和輸出層。設(shè)cij是C＝I+HT的第i行第j列元素,tik是S＝I-H的第i行第j列元素，則式(4)的計(jì)算機(jī)實(shí)現(xiàn)形式為(5)：圖4(a)為應(yīng)用本發(fā)明進(jìn)行容錯(cuò)控制的SY-2開架水下機(jī)器人，圖4(b)為其十字交叉形式的推進(jìn)器布置圖。圖4(b)中x0，y0表示水平面坐標(biāo)系。左、右主推螺旋槳分別固定在框架的兩側(cè)分別距離幾何中心為r1和r2，艏、艉側(cè)推螺旋槳分別固定在框架的前側(cè)和后側(cè)的正中間分別距離幾何中心為為r3和r4，四個(gè)螺旋槳形成了冗余的，容錯(cuò)的，可靠的，合理的水平推力布置，控制機(jī)器人完成水平面的三自由度運(yùn)動(dòng)。結(jié)合圖5，圖(a)和(b)分別是根據(jù)SY-2水下機(jī)器人所對(duì)應(yīng)的推進(jìn)器敞水試驗(yàn)得到的正車推力曲線和倒車推力曲線，推進(jìn)器為TECNADYNE的Model250。通過多次埃特金插值，可以利用該實(shí)驗(yàn)結(jié)果將推力分配結(jié)果轉(zhuǎn)化為控制電壓，實(shí)現(xiàn)對(duì)水下機(jī)器人水平面運(yùn)動(dòng)的控制。圖6是利用本發(fā)明進(jìn)行SY-2水下機(jī)器人容錯(cuò)控制的仿真結(jié)果曲線。圖中比較了兩種情況：a.推進(jìn)器無故障的情況；b.左主推40％故障且艉側(cè)推完全故障時(shí)的情況。機(jī)器人的期望運(yùn)動(dòng)軌跡為[0,0]→[85,130]→[0,200]→[150,200]→[150,130]→[0,0]。由仿真結(jié)果可知，本發(fā)明所設(shè)計(jì)的基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法可以在推進(jìn)器故障時(shí)重新進(jìn)行推力分配，雖然在前進(jìn)方向轉(zhuǎn)變時(shí)有些誤差，但基本也可完成故障時(shí)的點(diǎn)位軌跡控制。圖7描述了是某無人水下航行器-機(jī)械手系統(tǒng)及其垂向推進(jìn)器布置圖，系統(tǒng)包含一個(gè)4自由度的水下機(jī)械手，本文將利用本發(fā)明的方法實(shí)現(xiàn)手艇協(xié)調(diào)控制時(shí)的推力優(yōu)化分配。機(jī)械手每個(gè)關(guān)節(jié)的旋轉(zhuǎn)角度定義為θ1，θ2，θ3和θ4，包含肩部的擺動(dòng)關(guān)節(jié)和俯仰關(guān)節(jié)，肘部的俯仰關(guān)節(jié)和腕部的回轉(zhuǎn)關(guān)節(jié)。水下航行器配有4個(gè)垂推，垂向推進(jìn)器采用四邊形的布置形式，推進(jìn)器距離航行器的幾何中心為rvi，i＝1～4。圖8，為完成機(jī)械手的作業(yè)任務(wù)，無人水下航行器的控制器根據(jù)機(jī)械手的期望運(yùn)動(dòng)軌跡規(guī)劃了一段AUV的運(yùn)動(dòng)軌跡，以期在作業(yè)過程中盡量減少機(jī)械手給無人水下航行器造成的恢復(fù)力(矩)和耦合作用力。圖8(a)為機(jī)械手各關(guān)節(jié)的運(yùn)動(dòng)情況，圖8(b)為規(guī)劃得到的無人水下航行器軌跡、機(jī)械手肩關(guān)節(jié)(基關(guān)節(jié))軌跡、肘關(guān)節(jié)軌跡和機(jī)械手末端軌跡曲線；仿真得到無人水下航行器實(shí)際軌跡、機(jī)械手肩關(guān)節(jié)(基關(guān)節(jié))實(shí)際軌跡、肘關(guān)節(jié)實(shí)際軌跡和機(jī)械手末端實(shí)際軌跡曲線，仿真曲線是由無人水下航行器的控制算法結(jié)合本發(fā)明的基于雙重判據(jù)的水下機(jī)器人推力優(yōu)化分配方法分配垂向推力，同時(shí)控制航行器姿態(tài)和位移加以實(shí)現(xiàn)的。結(jié)合圖9，無人水下航行器-機(jī)械手系統(tǒng)在作業(yè)的過程中，機(jī)械手會(huì)給無人水下航行器帶來“擾動(dòng)力(力矩)”，包括作業(yè)過程中浮心和重心變化派生的恢復(fù)力矩，無人水下航行器和機(jī)械手的耦合性造成的拖拽力?？刂破鞲鶕?jù)系統(tǒng)的動(dòng)力學(xué)模型，由無人水下航行器和機(jī)械手的實(shí)時(shí)運(yùn)動(dòng)姿態(tài)計(jì)算出機(jī)械手對(duì)無人水下航行器在的實(shí)時(shí)干擾力，即為圖9觀測(cè)到的干擾力和干擾力矩。由圖可得機(jī)械手對(duì)水下航行器的擾動(dòng)力與擾動(dòng)力矩剛開始的時(shí)候出現(xiàn)較大抖動(dòng)，隨著時(shí)間的推移，橫向力趨于零，縱向力，垂向力與轉(zhuǎn)艏力矩趨于平穩(wěn)，橫傾力矩與轉(zhuǎn)艏力矩逐漸增大并趨于平穩(wěn)。說明本發(fā)明提出的基于雙重判據(jù)的水下航行器推力優(yōu)化分配方法能夠合理地分配垂向推力，從而實(shí)現(xiàn)水下航行器作業(yè)過程中的的姿態(tài)穩(wěn)定控制。