本發(fā)明涉及一種仿原鴿歸巢行為的無人機(jī)自主尋的控制方法，屬于無人機(jī)自主控制領(lǐng)域。

背景技術(shù)：

1、目前，使用無人機(jī)進(jìn)行搜索和尋的(dì)引起了廣泛的關(guān)注。在現(xiàn)實(shí)生活中，已有將無人機(jī)應(yīng)用于災(zāi)后搜救和目標(biāo)搜索打擊等內(nèi)容。在可以預(yù)見的未來，也會(huì)發(fā)展出無人機(jī)外送相關(guān)服務(wù)。在進(jìn)行相關(guān)任務(wù)的同時(shí)，無人機(jī)需要到達(dá)一個(gè)指定的地點(diǎn)，面臨著復(fù)雜環(huán)境帶來的種種挑戰(zhàn)，例如樹木或者電線等障礙物妨害無人機(jī)的正常飛行。在相關(guān)的環(huán)境中，無人機(jī)需要具備自主尋的能力以適應(yīng)各種復(fù)雜的地形和環(huán)境特征。自主尋的使得無人機(jī)能夠適應(yīng)多樣化的環(huán)境，避免與環(huán)境障礙物發(fā)生碰撞，并安全地導(dǎo)航到目標(biāo)位置。

2、無人機(jī)的尋的問題目前已經(jīng)得到了廣泛討論，從路徑規(guī)劃，路徑跟蹤等方面都已經(jīng)有了非常廣泛的討論。但是相關(guān)討論主要集中在頂層的應(yīng)用設(shè)計(jì)之上，在具體的無人機(jī)控制方面，如何將控制方法落實(shí)到無人機(jī)的控制上，討論仍然有所不足。通過將所規(guī)劃出的路徑進(jìn)行時(shí)間參數(shù)化，形成一條無人機(jī)的動(dòng)力學(xué)可行路徑是將頂層的路徑設(shè)計(jì)下沉到無人機(jī)具體控制的一個(gè)很好方法。通過時(shí)間參數(shù)化的路徑設(shè)計(jì)，所形成的路徑具有可行性強(qiáng)，調(diào)整靈活等一系列優(yōu)點(diǎn)，是未來無人機(jī)尋的控制的一條較好的技術(shù)路線。

3、目前，針對(duì)無人機(jī)的自主避障的問題，常用的避障方法包括有人工勢(shì)場法為代表的一系列方法。這種方法使用方便，應(yīng)用廣泛，但是其缺乏一定的智能性，在面對(duì)復(fù)雜環(huán)境時(shí)容易出現(xiàn)陷入局部最優(yōu)的困境。另一種主要方法為速度障礙法，但是其計(jì)算復(fù)雜度高，計(jì)算開銷很大。

4、鴿子在歸巢時(shí)，體現(xiàn)出優(yōu)良的導(dǎo)航性能和路徑優(yōu)化能力，將原鴿歸巢行為融入無人機(jī)的自主尋的過程中，能夠有效提升無人機(jī)尋的的智能性，進(jìn)而提升無人機(jī)的適應(yīng)性和靈活性，使其能夠較好地適應(yīng)無人機(jī)的實(shí)際任務(wù)場景，靈活調(diào)整無人機(jī)尋的行為，實(shí)現(xiàn)無人機(jī)自主尋的。

5、綜上所述，本發(fā)明針對(duì)目前無人機(jī)集群在復(fù)雜環(huán)境中自主尋的問題，考慮環(huán)境中的障礙物，采取一種仿原鴿歸巢行為的無人機(jī)自主尋的控制方法對(duì)無人機(jī)進(jìn)行自主地控制。該方法通過使用多項(xiàng)式插值的方法，平滑無人機(jī)的原始規(guī)劃路徑，實(shí)現(xiàn)無人機(jī)尋的過程中的后端控制。同時(shí)，從原鴿歸巢行為中獲得啟發(fā)，通過模仿原鴿歸巢行為，實(shí)現(xiàn)無人機(jī)躲避環(huán)境障礙并到達(dá)尋的目標(biāo)。這種方法實(shí)用性強(qiáng)，適應(yīng)性好，能夠?qū)崿F(xiàn)無人機(jī)的自主尋的。

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

1、1、發(fā)明目的：

2、本發(fā)明一種仿原鴿歸巢行為的無人機(jī)自主尋的控制方法，其目的是提供一種提升無人機(jī)的自主能力水平，實(shí)現(xiàn)復(fù)雜環(huán)境下無人機(jī)的自主尋的，并且能夠主動(dòng)避開環(huán)境中的障礙物，該方法旨在提高無人機(jī)在各種任務(wù)中對(duì)不同環(huán)境的避碰能力，并設(shè)計(jì)與之相匹配的無人機(jī)控制方法，從而提升無人機(jī)在復(fù)雜環(huán)境中的適應(yīng)性和自主性，提升無人機(jī)的自主能力水平。

3、2、技術(shù)方案：

4、本發(fā)明針對(duì)無人機(jī)在環(huán)境中對(duì)目標(biāo)的自主尋的問題，發(fā)明了一種仿原鴿歸巢行為的無人機(jī)自主尋的控制方法，流程圖如圖1所示，具體實(shí)現(xiàn)步驟如下：

5、步驟一：場景初始化

6、對(duì)無人機(jī)進(jìn)行建模，無人機(jī)建模為如下所示的雙積分系統(tǒng)：

7、

8、其中，p，v，u分別為無人機(jī)的位置、速度和加速度矢量。

9、隨后，初始化環(huán)境地圖，在環(huán)境中設(shè)置初始障礙物，設(shè)置每個(gè)障礙物的形狀，大小，中心點(diǎn)位置等參數(shù)。

10、步驟二：環(huán)境地圖柵格化

11、根據(jù)無人機(jī)尺寸，選取無人機(jī)的半徑作為膨脹參數(shù)，對(duì)障礙物進(jìn)行膨脹。然后，根據(jù)設(shè)置的柵格地圖分辨率，為場景地圖劃分網(wǎng)格。最后，檢查環(huán)境中障礙物在網(wǎng)格中的分布情況，如果在劃分某一個(gè)網(wǎng)格中存在障礙物，則將該網(wǎng)格的數(shù)值置為1；否則置為0。

12、步驟三：a*算法全局路徑規(guī)劃

13、根據(jù)步驟二中得到的柵格地圖，將無人機(jī)當(dāng)前所在的柵格位置作為規(guī)劃起始點(diǎn)，將需要到達(dá)的目標(biāo)的柵格位置作為終點(diǎn)，使用a*算法設(shè)計(jì)從起點(diǎn)到終點(diǎn)的原始路徑，該路徑由路徑上的一系列離散點(diǎn)表示，即全局路徑：γp＝[p1,p2…,pn]，其中，p1為無人機(jī)的初始位置，pn＝pa為無人機(jī)的目標(biāo)點(diǎn)，也為無人機(jī)的最后的路徑點(diǎn)，pi(i＝2,3,…,n-1)為無人機(jī)的中間路徑點(diǎn)。

14、其中，a*算法的目標(biāo)函數(shù)g(p)為地圖格點(diǎn)與目標(biāo)的歐氏距離，具體的計(jì)算公式如下：

15、g(p)＝||pa-p||?????(2)

16、其中，p為a*算法當(dāng)前遍歷的坐標(biāo)，pa為目標(biāo)的坐標(biāo)。

17、步驟四：基于多項(xiàng)式插值的參數(shù)化路徑設(shè)計(jì)

18、根據(jù)步驟三中得到的全局路徑γp，將該全局路徑參數(shù)化為可行的全局路徑，并通過多項(xiàng)式插值細(xì)化參數(shù)化路徑。具體步驟如下：

19、s41、時(shí)間參數(shù)化路徑初步設(shè)計(jì)

20、在步驟三設(shè)計(jì)出的全局路徑的基礎(chǔ)上，進(jìn)行時(shí)間參數(shù)化。首先給無人機(jī)初始位置設(shè)計(jì)一個(gè)參數(shù)化時(shí)間t1＝0，隨后根據(jù)無人機(jī)的最大速度vm，對(duì)路徑點(diǎn)序列(即全局路徑)γp中所設(shè)計(jì)的每一個(gè)空間路徑點(diǎn)(以下簡稱“路徑點(diǎn)”)設(shè)置一個(gè)合理的期望到達(dá)時(shí)間，進(jìn)而得到一條初始時(shí)間參數(shù)化路徑γt＝[(t1,p1),(t2,p2),…,(tn,pn)]。第i個(gè)路徑點(diǎn)的時(shí)間化參數(shù)計(jì)算方式如下：

21、

22、其中，為了方便后續(xù)的路徑插值處理，每兩個(gè)路徑點(diǎn)之間的時(shí)間差是一個(gè)定值。

23、s42、速度參數(shù)化路徑設(shè)計(jì)

24、根據(jù)得到的時(shí)間參數(shù)化路徑γt設(shè)置路徑終點(diǎn)的期望速度vn＝0，隨后采用后向差分的方法計(jì)算除終點(diǎn)外每一個(gè)路徑點(diǎn)處的期望速度，即參數(shù)化路徑速度，具體計(jì)算方式如下：

25、

26、其中ti,pi分別為第i個(gè)時(shí)間參數(shù)化路徑點(diǎn)的期望到達(dá)時(shí)間和路徑位置。

27、通過參數(shù)化路徑速度，得到速度參數(shù)化路徑γv＝[(t1,p1,v1),(t2,p2,v2),…,(tn,pn,vn)]。

28、s43、參數(shù)化路徑多項(xiàng)式插值

29、首先根據(jù)無人機(jī)的觀測間隔，細(xì)化無人機(jī)時(shí)間參數(shù)化路徑的時(shí)間基礎(chǔ)序列。然后，根據(jù)步驟s42中得到的速度參數(shù)化路徑γv，采用多項(xiàng)式擬合的方法，對(duì)每段路徑中的多個(gè)時(shí)間點(diǎn)進(jìn)行插值，確定時(shí)間基礎(chǔ)序列中每個(gè)插值時(shí)間點(diǎn)的速度，平滑無人機(jī)的速度變化。具體的設(shè)計(jì)方法如下：

30、對(duì)于s42中所設(shè)計(jì)的帶有n個(gè)路徑點(diǎn)的速度參數(shù)化路徑γv＝[(t1,p1,v1),(t2,p2,v2),…,(tn,pn,vn)]，在每兩個(gè)時(shí)刻之間，設(shè)計(jì)一系列插值時(shí)間點(diǎn)ti,j，并和時(shí)間參數(shù)化路徑中的期望到達(dá)時(shí)間序列一起作為時(shí)間基礎(chǔ)序列t′＝[t1,t1,1,t1,2,…,t2,t2,1,…,tn]＝[t′1,t′2,…,t′n]。設(shè)每兩個(gè)時(shí)間之間具有n個(gè)插值點(diǎn)，則有對(duì)應(yīng)關(guān)系式(5)：

31、

32、其中,ti,j的計(jì)算公式如下：

33、

34、其中，n為每兩個(gè)時(shí)間點(diǎn)中的插值點(diǎn)的個(gè)數(shù)。

35、同樣采用線性插值的方法，得到每個(gè)時(shí)間基礎(chǔ)序列中每個(gè)插值時(shí)刻所對(duì)應(yīng)的路徑點(diǎn)位置坐標(biāo)p′i，并和全局路徑γp中的路徑點(diǎn)序列一起作為插值路徑中的路徑點(diǎn)序列p′＝[p1,p1,1,p1,2,…,p2,p2,1,…,pn]＝[p′1,p′2,…,p′n]。具體的計(jì)算公式如下：

36、

37、p′i與pi,j的對(duì)應(yīng)關(guān)系如下：

38、

39、隨后，根據(jù)原本的速度參數(shù)化路徑γv，使用三次多項(xiàng)式插值的方法計(jì)算時(shí)間基礎(chǔ)序列對(duì)應(yīng)的速度序列v′＝[v′1,v′2,…,v′n]，具體的計(jì)算方法如下：

40、首先，選取速度參數(shù)化路徑γv的前四個(gè)時(shí)間點(diǎn)t1,t2,t3,t4，每三個(gè)時(shí)間點(diǎn)為一組，計(jì)算每三個(gè)時(shí)間點(diǎn)的三次多項(xiàng)式速度擬合函數(shù)f1(t)和f2(t)。每個(gè)三次多項(xiàng)式函數(shù)具有如下形式：f(t)＝a3t3+a2t2+a1t+a0。

41、每個(gè)時(shí)間點(diǎn)的速度應(yīng)當(dāng)與速度參數(shù)化路徑中設(shè)計(jì)的速度相等，并且兩個(gè)多項(xiàng)式相交處的一階導(dǎo)數(shù)(加速度)應(yīng)當(dāng)相等。根據(jù)上述條件，列出如下8個(gè)方程：

42、

43、通過上述8個(gè)方程，解得到兩個(gè)多項(xiàng)式函數(shù)的8個(gè)系數(shù)，并根據(jù)時(shí)間基礎(chǔ)序列t′中每點(diǎn)的時(shí)刻，計(jì)算對(duì)應(yīng)時(shí)刻的期望速度。具體計(jì)算公式如下：

44、

45、對(duì)于速度參數(shù)化路徑γv的第i個(gè)點(diǎn)(i≥5)，令第i-1個(gè)點(diǎn)滿足的速度擬合函數(shù)為fi-3(t)，已經(jīng)經(jīng)過求解得到，則通過下列方程求解得到時(shí)間t(ti-1<t≤ti)時(shí)間段上的速度擬合函數(shù)fi(t)：

46、

47、并通過上述擬合函數(shù)計(jì)算時(shí)間基礎(chǔ)序列中每個(gè)時(shí)刻t′j的速度：

48、v′j＝fi-2(t′j)(ti-1<t′j≤ti)?????(12)

49、綜合上述的時(shí)間【式(5)(6)】、位置【式(7)，(8)】、速度【式(10)，(12)】的插值結(jié)果t′，p′和v′，組合得到插值后的路徑γ＝[(t′1,p′1,v′1),(t′2,p′2,v′2),…,(t′n,p′n,v′n)]，即為最終的插值路徑。

50、步驟五：仿原鴿歸巢行為的路徑引導(dǎo)

51、首先，基于步驟四所設(shè)計(jì)的插值后的插值路徑γ，設(shè)計(jì)無人機(jī)自主尋的基礎(chǔ)控制項(xiàng)。具體計(jì)算如下所示：

52、ub＝kp(p′i-p)+kv(v′i-v)?(13)

53、其中，kp，kv分別為位置和速度控制增益；pi，vi分別為時(shí)間參數(shù)化路徑中每一段所對(duì)應(yīng)的位置和速度；p，v分別為無人機(jī)的位置、速度矢量；i表示無人機(jī)當(dāng)前所處的時(shí)間參數(shù)化路徑的編號(hào)。

54、隨后基于原鴿歸巢行為，設(shè)計(jì)無人機(jī)尋的控制修正項(xiàng)。具體步驟如下：

55、原鴿在進(jìn)行歸巢時(shí)，會(huì)遵循歸巢的相關(guān)原則。將其映射到無人機(jī)的尋的過程中，主要包括兩個(gè)方面：第一，鴿巢對(duì)鴿群具有吸引作用，將其映射為無人機(jī)對(duì)目標(biāo)點(diǎn)的趨向性。第二，原鴿在歸巢時(shí)會(huì)躲避地形障礙物或者捕食者的攻擊，將其映射為無人機(jī)對(duì)環(huán)境中障礙物的規(guī)避行為。基于上述兩個(gè)方面，設(shè)計(jì)仿原鴿歸巢行為的路徑引導(dǎo)修正如下：

56、s51、巢穴吸引項(xiàng)

57、無人機(jī)對(duì)尋的目標(biāo)點(diǎn)進(jìn)行貪婪式前進(jìn)，相應(yīng)的控制修正量為：

58、

59、其中，ua為無人機(jī)朝向目標(biāo)運(yùn)動(dòng)的加速度修正，pa為目標(biāo)點(diǎn)的位置矢量，p為無人機(jī)的位置矢量，ka為巢穴吸引項(xiàng)的加速度增益。

60、s52、捕食避障項(xiàng)

61、無人機(jī)在面對(duì)環(huán)境中障礙或者進(jìn)攻者時(shí)，為了躲避而產(chǎn)生捕食避障加速度。本發(fā)明從原鴿躲避鷹群的過程中得到啟發(fā)，實(shí)現(xiàn)無人機(jī)尋的自主避障控制。

62、首先使用采樣的方式，遍歷柵格地圖中的每一個(gè)格子，對(duì)于存在障礙物的格子，記錄該格子的中心坐標(biāo)作為障礙物的點(diǎn)云坐標(biāo)。通過整合整個(gè)地圖的障礙物點(diǎn)云坐標(biāo)，得到環(huán)境中障礙物的點(diǎn)云信息。

63、利用所有的障礙物點(diǎn)云信息，計(jì)算無人機(jī)尋的避障項(xiàng)如下所示：

64、

65、其中，ud表示無人機(jī)因?yàn)楸苷隙a(chǎn)生的總的加速度，ks表示無人機(jī)為了躲避第s個(gè)障礙點(diǎn)云而產(chǎn)生的加速度增益，rc表示無人機(jī)的安全距離，ps表示第s個(gè)障礙物點(diǎn)云的位置矢量。

66、s53、無人機(jī)仿原鴿歸巢運(yùn)動(dòng)狀態(tài)更新

67、基于上述s51、s52的原則，可以設(shè)計(jì)無人機(jī)的控制量，無人機(jī)受到的總的加速度為u如下所示，帶入無人機(jī)控制模型，即可更新無人機(jī)運(yùn)動(dòng)狀態(tài)。

68、u＝ub+ua+ud?(17)

69、步驟六：無人機(jī)尋的路徑和尋的結(jié)果

70、根據(jù)所設(shè)計(jì)的無人機(jī)路徑和無人機(jī)自主尋的控制方法，控制無人機(jī)到達(dá)指定位置，展示無人機(jī)尋的路徑和尋的結(jié)果。

71、3、優(yōu)點(diǎn)及效果：

72、本發(fā)明提出了一種仿原鴿歸巢行為的無人機(jī)自主尋的控制方法，本發(fā)明的主要具有如下優(yōu)勢(shì)：一、發(fā)明一種完整的無人機(jī)自主尋的方法，涵蓋無人機(jī)自主尋的前端和后端技術(shù)方案，方法新穎全面實(shí)用，能夠滿足無人機(jī)在較復(fù)雜環(huán)境下的自主尋的和避障任務(wù)；二、基于多項(xiàng)式插值的方法，對(duì)規(guī)劃出的原始路徑進(jìn)行插值處理，實(shí)現(xiàn)了路徑規(guī)劃從前端到后端的過渡，并且完成了過程中速度的平滑，避免了由于不同路徑段過渡狀態(tài)中由于速度突變?cè)斐傻能壽E突變；三、通過模仿原鴿歸巢行為，建立了無人機(jī)自主控制方案，導(dǎo)引無人機(jī)到達(dá)指定目標(biāo)，具有實(shí)時(shí)性好，適應(yīng)性強(qiáng)等優(yōu)點(diǎn)，能夠?qū)崿F(xiàn)對(duì)環(huán)境良好的適應(yīng)。