本發(fā)明涉及一種基于跟隨領(lǐng)航者法的航速航向協(xié)同控制編隊航行控制方法，屬于船舶自動控制技術(shù)領(lǐng)域。

背景技術(shù)：

多智能體的編隊協(xié)同控制問題，是機器人和船舶智能控制領(lǐng)域的研究熱點。編隊控制包括編隊方法、隊形形成控制和隊形保持控制三方面內(nèi)容。編隊方法解決了多智能體以何種隊形同步航行的問題。隊形形成控制是將多只船從開始的隨機位置形成規(guī)定的整體隊形的過程。隊形保持控制是整體隊形形成后，如何實現(xiàn)隊形不再錯亂的控制過程。

目前，編隊航行控制尚處于理論研究階段，對于多機器人編隊問題的研究主要有以下方法：leader-follower法(跟隨領(lǐng)航者法)、人工勢函數(shù)法等。其中，leader-follower法是一種編隊方法，不解決如何形成編隊的問題。人工勢函數(shù)法主要用于解決隊形形成控制，無法隊形保持控制。

而常規(guī)的leader-follower法和人工勢場法，從控制機理上決定了僅能實現(xiàn)船舶位置收斂控制，而無法在期望位置點上實現(xiàn)領(lǐng)航者和跟隨者的航速航向一致性同步控制，導(dǎo)致無法以穩(wěn)定編隊狀態(tài)同步航行，因此只能實現(xiàn)船舶編隊的隊形形成控制，而無法實現(xiàn)后續(xù)的隊形保持控制，其控制過程與控制機理如圖1所示。

如圖1所示，人工勢場法首先由khatib提出，其基本思想是借鑒物理方面的概念，目標(biāo)對船舶產(chǎn)生吸引力，從而引導(dǎo)船舶向目標(biāo)運動。以跟隨者r2為例，在編隊航行過程中，受到編隊位置t2的吸引，從而驅(qū)動船舶向t2點駛?cè)?。由于?qū)動方向始終由船舶當(dāng)前位置點r2指向t2，人工勢場法可以實現(xiàn)控制船舶逼近t2點，但未能提供有效的航速和航向約束，因此無法實現(xiàn)以特定速度和航向到達t2點，也就無法實現(xiàn)后續(xù)的隊形保持控制。另外，當(dāng)船舶r2到達目標(biāo)點t2附近時，由于持續(xù)的吸引力，將極可能導(dǎo)致船舶超越目標(biāo)點，使之出現(xiàn)目標(biāo)點位于船舶身后的情況，誘發(fā)船舶發(fā)生極大的轉(zhuǎn)向動作或不可預(yù)知的航行動作，安全隱患很大。

綜上所述，當(dāng)前編隊控制算法研究主要集中于編隊方法和編隊形成的算法，忽視了編隊形成后的隊形保持控制。人工勢場法是一種十分有效的編隊形成控制方法，但無法同步實現(xiàn)隊形形成和隊形保持兩方面的控制需求。

技術(shù)實現(xiàn)要素：

為解決現(xiàn)有技術(shù)的不足，本發(fā)明的目的在于提供一種基于跟隨領(lǐng)航者法的航速航向協(xié)同控制編隊航行控制方法，解決目前編隊形成后的隊形保持控制方法不完善，隊形保持效果很差的問題。

為了實現(xiàn)上述目標(biāo)，本發(fā)明采用如下的技術(shù)方案：

一種基于跟隨領(lǐng)航者法的航速航向協(xié)同控制編隊航行控制方法，其特征是，所述船舶編隊包括領(lǐng)航者和跟隨者，領(lǐng)航者的速度和航向記為(v1,ψ1)，跟隨者的速度和航向記為(v2,ψ2)，其中：

領(lǐng)航者航行控制方法包括如下步驟：

步驟1-1)設(shè)置領(lǐng)航者需要跟蹤(t11,t12)航跡點序列，其中，t11的坐標(biāo)記為(xd1,yd1)，t12的坐標(biāo)記為(xd2,yd2)，并計算目標(biāo)航線角ψd；

步驟1-2)計算領(lǐng)航者距離目標(biāo)航線t11-t12的垂直距離d11；

步驟1-3)通過gps及航姿測量設(shè)備獲取船舶的當(dāng)前航向角ψ1，通過航向角ψ1與目標(biāo)航線角ψd之間的偏差，計算出舵角控制回路的期望舵角δc；

步驟1-4)通過舵角傳感器獲取船舶的舵角信息，計算當(dāng)前舵角值與步驟1-3)計算得到的期望舵角之間的偏差，再利用船舶上的舵角執(zhí)行機構(gòu)調(diào)整船舶舵的方位；

跟隨者控制方法包括如下步驟：

步驟2-1)跟隨者接收領(lǐng)航者的位置、航向角ψ1和速度v1信息；

步驟2-2)根據(jù)領(lǐng)航者的位置和跟隨者自身的位置信息，根據(jù)編隊隊形形狀計算跟隨者距離期望位置t21的橫向距離誤差d21和縱向距離誤差d22，無需計算t21的具體坐標(biāo)值；

步驟2-3)計算跟隨者的驅(qū)動速度v2，其中，v2是與縱向距離誤差d22和v1有關(guān)的函數(shù)，當(dāng)d22→0時，v2→v1；

步驟2-4)計算跟隨者的驅(qū)動航向ψ2，其中，ψ2是與橫向距離誤差d21和ψ1有關(guān)的函數(shù)，當(dāng)d21→0時，ψ2→ψ1；

步驟2-5)通過控制油門和舵角使d22→0和d21→0，跟隨者的航速和航向?qū)㈦S之收斂于(v1,ψ1)，即跟隨者的速度和航向與領(lǐng)航者保持一致，從而實現(xiàn)編隊航行隊形形成和保持控制。

進一步地，所述步驟1-1)中目標(biāo)航線角ψd的計算公式為

其中，為目標(biāo)航線角ψd的過渡計算變量，xd1為目標(biāo)點t11的經(jīng)度值，yd1為目標(biāo)點t11的緯度值，xd2為目標(biāo)點t12的經(jīng)度值，yd2為目標(biāo)點t12的緯度值，c為船舶目標(biāo)點t11、目標(biāo)點t12與地心所構(gòu)成的地心角，cos(c)為其余弦值。

進一步地，所述步驟1-1)中需要根據(jù)船舶目標(biāo)點t12與t11的相對位置關(guān)系進行象限轉(zhuǎn)換，轉(zhuǎn)換方式如下：

以目標(biāo)點t11為原點，建立直角坐標(biāo)系，其中正北方(緯度方向)為縱軸，與縱軸垂直的正東方(經(jīng)度方向)為橫軸。

若目標(biāo)點t12位于該直角坐標(biāo)系的第一象限，則

若目標(biāo)點t12位于該直角坐標(biāo)系的第二象限，則

若目標(biāo)點t12位于該直角坐標(biāo)系的第三四象限，則

進一步地，所述步驟1-3)中計算出舵角控制回路的期望舵角δc時利用s函數(shù)進行計算，其中s函數(shù)如下：其中，y為控制量，e為控制誤差，k為曲線斜率，max代表控制最大值，其中k、max的取值可根據(jù)實際控制機構(gòu)的性能以及經(jīng)驗值確定。

進一步地，所述步驟1-3)中期望舵角δc＝f1+f2，其中，f1為主舵角控制量；f2為航跡收斂控制舵角修正量，在航向控制的基礎(chǔ)上，控制船舶向期望航線t11-t12收斂。

進一步地，所述步驟1-3)中f1＝f(ψd-ψ1)，采用經(jīng)典pid控制方法進行計算。

進一步地，所述步驟1-3)中f2采用s函數(shù)計算：

進一步地，所述步驟2-3)中跟隨者的驅(qū)動速度

d22→0，δv→0，v2→v1。

進一步地，所述步驟2-4)中跟隨者的驅(qū)動航向

d21→0，δψ→0，ψ2→ψ1。

本發(fā)明所達到的有益效果：通過在編隊形成過程中加入速度和航向控制約束，適用于編隊控制中的隊形形成和隊形保持控制，具有很強的工程操作性和實施性，無需人工干預(yù)，可廣泛用以解決于無人值守環(huán)境下的船舶編隊航行控制問題，尤其適用于水面靶場、高危水域等場合的船舶編隊航行需要。

附圖說明

圖1是現(xiàn)有技術(shù)中人工勢場法的原理圖；

圖2是本發(fā)明的控制原理圖；

圖3是s函數(shù)不同斜率下的仿真結(jié)果；

圖4是船舶編隊控制工作原理圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明作進一步描述。以下實施例僅用于更加清楚地說明本發(fā)明的技術(shù)方案，而不能以此來限制本發(fā)明的保護范圍。

要實現(xiàn)船舶的穩(wěn)定編隊航行控制，無論采用哪種具體的控制方法，均需通過調(diào)節(jié)船舶的航速和航向，最終實現(xiàn)位置、航速、航向等多種狀態(tài)的同步收斂控制。只有在同一時刻實現(xiàn)位置、航速和航向的同步收斂控制，才能實現(xiàn)多船由無緒狀態(tài)到有緒狀態(tài)，并保持有緒狀態(tài)航行的控制過程。leader-follower法為例，在編隊形成過程中，需要跟隨者到達期望隊形位置時，應(yīng)具有與領(lǐng)航者相同的速度和航向，方能實現(xiàn)后續(xù)的隊形保持控制。

針對當(dāng)前主流編隊控制方法所存在的使用局限性，即忽視了隊形保持控制的重要性和實現(xiàn)方法，而且在編隊航行過程中，隊形保持一般是歷時時間最長、最為重要的一種穩(wěn)定航行狀態(tài)。

編隊航行控制的本質(zhì)控制問題，是在同一時刻實現(xiàn)位置、航速和航向的同步收斂控制，基于此分析本發(fā)明將編隊控制過程分解為速度控制回路和航向控制回路，位置縱向誤差和航速調(diào)節(jié)由速度控制回路負責(zé)，位置側(cè)向誤差和航向調(diào)節(jié)由航向控制回路解決，以避免常規(guī)編隊控制方法在隊形保持過程中航向控制參與調(diào)節(jié)位置縱向誤差的危險動作，導(dǎo)致無法實現(xiàn)隊形保持甚至引發(fā)極大的轉(zhuǎn)向動作或不可預(yù)知的航行動作等危險情況。

本發(fā)明專利是針對高速靶船的編隊航行控制需求而提出的。在軍用靶船領(lǐng)域，為逼真模擬實際戰(zhàn)場環(huán)境的復(fù)雜電磁環(huán)境特性和被打擊對象的運動特性，需要由目標(biāo)靶船、輻射源船、干擾源船等組成靶船編隊，以逼真模擬敵航母、驅(qū)逐艦、護衛(wèi)艦等航母戰(zhàn)斗群的編隊航行運動狀態(tài)，以及敵艦載雷達武器裝備的輻射特性和干擾特性。其中，目標(biāo)靶船為演練中被攻擊摧毀的靶船，輻射源船和干擾源船為任務(wù)配試船只，搭載貴重的任務(wù)設(shè)備(如輻射源、干擾源、圖像及通信設(shè)備等)，在靶試訓(xùn)練中重復(fù)使用，以降低靶試成本。同樣地，本發(fā)明也適用于常規(guī)船舶、機器人等多智能體的編隊協(xié)同控制問題，是一種通用的編隊控制方法。

區(qū)別于常規(guī)的leader-follower法，領(lǐng)航者、跟隨者各自向預(yù)定好的期望位置驅(qū)動控制，本發(fā)明將跟隨者的期望位置根據(jù)領(lǐng)航者實時確定，從航行開始即讓跟隨者始終接受編隊隊形的約束作用，快速進入編隊狀態(tài)，并以預(yù)定隊形的方式共同向目標(biāo)點t11前進。在實際工程應(yīng)用中，目標(biāo)點由一系列點組成，在不影響對專利內(nèi)容理解的基礎(chǔ)上，為表述方便，以單點代表目標(biāo)點序列。

如圖2所示，本案中，船舶編隊由一個領(lǐng)航者r1、多個跟隨者(r2、r3…,rn)組成，所有跟隨者的控制方法步驟完成相同，在本發(fā)明中以單個跟隨者r2表示。領(lǐng)航者的速度和航向記為(v1,ψ1)，跟隨者的速度和航向記為(v2,ψ2)。對于每一個跟隨者，將當(dāng)前位置與目標(biāo)位置的誤差分解為位置縱向誤差和位置側(cè)向誤差，第i個船舶的位置縱向誤差為di2(i＝1,2,…,n)，位置側(cè)向誤差為di1(i＝1,2,…,n)，n為船舶總個數(shù)。在本權(quán)利要求中，對于第i個船舶的位置縱向誤差為di2(i＝1,2,…,n)以及位置側(cè)向誤差為di1(i＝1,2,…,n)，以d21和d22為說明，也就是以第2個船舶進行了說明，對于之后的第n個船舶，其原理與第2個完全一樣。

在本方案中，采用了一種斜率幅度可調(diào)的s函數(shù)，該函數(shù)可以限定控制邊界并提供正確的趨勢控制響應(yīng)，所提出的s函數(shù)如下：其中，y為控制量，e為控制誤差，k為曲線斜率，max代表控制最大值。

以max＝1為，k分別取0.01和0.1時的函數(shù)曲線如圖3所示。

在進行具體控制時：

領(lǐng)航者可以固定速度v1(v1<vmax)航行，通過進行航向控制，實現(xiàn)航跡收斂，vmax表示領(lǐng)航者的限速上限，其航行控制方法包括以下計算步驟：

步驟1-1)假設(shè)領(lǐng)航者需要跟蹤(t11,t12)航跡點序列，t11的坐標(biāo)記為(xd1,yd1)，t12的坐標(biāo)記為(xd2,yd2)，首先計算目標(biāo)航線角ψd：

計算公式為：其中，xd1為目標(biāo)點t11的經(jīng)度值，yd1為目標(biāo)點t11的緯度值；xd2為目標(biāo)點t12的經(jīng)度值，yd2為目標(biāo)點t12的緯度值，c為船舶目標(biāo)點t11、目標(biāo)點t12與地心所構(gòu)成的地心角；為目標(biāo)航線角ψd的過渡計算變量，還需要根據(jù)船舶目標(biāo)點t12與t11的相對位置關(guān)系進行象限轉(zhuǎn)換：

以目標(biāo)點t11為原點，建立直角坐標(biāo)系，其中正北方(緯度方向)為縱軸，與縱軸垂直的正東方(經(jīng)度方向)為橫軸。

若目標(biāo)點t12位于該直角坐標(biāo)系的第一象限，則

若目標(biāo)點t12位于該直角坐標(biāo)系的第二象限，則

若目標(biāo)點t12位于該直角坐標(biāo)系的第三四象限，則

步驟1-2)：計算領(lǐng)航者r1距離目標(biāo)航線t11-t12的垂直距離d11，該計算方法為常規(guī)算法，比如直接通過坐標(biāo)進行計算；

步驟1-3)：通過gps及航姿測量設(shè)備獲取船舶的當(dāng)前航向角ψ1，通過航向角與目標(biāo)航線角ψd之間的偏差，并利用s函數(shù)計算出舵角控制回路的期望舵角δc；其中，f1為主舵角控制量，可選用經(jīng)典pid控制方法或其它類似控制方法；f2為航跡收斂控制舵角修正量，在航向控制的基礎(chǔ)上，控制船舶向期望航線t11-t12收斂，f2采用s函數(shù)計算；

步驟1-4)通過舵角傳感器獲取船舶的舵角信息，計算當(dāng)前舵角值與步驟1-3)計算得到的期望舵角之間的偏差，再利用船舶上的舵角執(zhí)行機構(gòu)調(diào)整船舶舵的方位。

以跟隨者r2為例，通過調(diào)節(jié)速度v2和ψ2，使其位置點收斂至t21，同時將速度v2和航向ψ2分別收斂至(v1,ψ1)，跟隨者控制方法包括以下計算步驟：

步驟2-1)跟隨者r2接收領(lǐng)航者r1的位置、航向ψ1和速度v1信息；

步驟2-2)根據(jù)領(lǐng)航者r1的位置和跟隨者r2自身的位置信息，計算跟隨者r2距離期望位置t21的橫向距離誤差d21和縱向距離誤差d22，具體計算均為簡單的幾何運算，此處不再贅述；

步驟2-3)計算跟隨者r2的驅(qū)動速度，如下：

d22→0，δv→0，v2→v1；

步驟2-4)計算跟隨者r2的驅(qū)動航向，如下：

d21→0，δψ→0，ψ2→ψ1。

步驟2-5)通過控制油門和舵角使跟隨者r2的航速和航向分別收斂于(v1,ψ1)。

以上公式中，→表示收斂的意思，在船舶控制領(lǐng)域，屬于通用屬于，進一步地指代將對應(yīng)的航向角或者速度向某一值趨近直至一樣或者在誤差范圍內(nèi)。

如圖4所示，為實現(xiàn)本發(fā)明所提供的編隊航行控制算法，需要領(lǐng)航者與跟隨者之間具備無線通信能力，領(lǐng)航者向跟隨者發(fā)送領(lǐng)航者位置、參考速度和參考航向等信息。無論對于領(lǐng)航者還是跟隨者，一種典型但不限于此的控制系統(tǒng)配置包括無線通信電臺、航行控制計算機、航向航速控制板、發(fā)動機、舵角執(zhí)行機構(gòu)、gps及航姿測量設(shè)備、舵以及舵角傳感器。

領(lǐng)航者r1之航行控制計算機通過無線通信電臺接收期望航跡點序列和航速等遠程控制指令，以實現(xiàn)指定速度和航跡領(lǐng)隊航行；

跟隨者r2之航行控制計算機通過無線通信電臺接收領(lǐng)航者r1的當(dāng)前位置、參考速度和參考航向，實時計算驅(qū)動速度和驅(qū)動航向，控制跟隨者以預(yù)設(shè)隊形與領(lǐng)航者同步航行；跟隨者r3同理控制；

航行控制計算機通過rs232/rs422串口接收gps及航姿測量設(shè)備輸出的船舶位置及航向信息，并進行航向控制回路解算和航速控制回路解算；

航速航向控制板通過網(wǎng)絡(luò)接收航行控制機輸出的航速和航向指令，并采集舵角反饋信息，計算當(dāng)前舵角值與計算得到的期望舵角之間的偏差，解算后通過控制總線輸出油門模擬電壓和啟動指令至發(fā)動機，并發(fā)送舵角執(zhí)行指令至舵角執(zhí)行機構(gòu)，由舵角執(zhí)行機構(gòu)控制舵的方向。

以上所述僅是本發(fā)明的優(yōu)選實施方式，應(yīng)當(dāng)指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明技術(shù)原理的前提下，還可以做出若干改進和變形，這些改進和變形也應(yīng)視為本發(fā)明的保護范圍。