本發(fā)明屬于路徑規(guī)劃，具體涉及一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法。

背景技術(shù)：

1、隨著無(wú)人機(jī)技術(shù)的發(fā)展，無(wú)人機(jī)在軍事偵察、災(zāi)害搜救、環(huán)境監(jiān)測(cè)等領(lǐng)域的應(yīng)用越來(lái)越廣泛。動(dòng)態(tài)目標(biāo)追蹤是無(wú)人機(jī)自主導(dǎo)航的一項(xiàng)關(guān)鍵技術(shù)，要求無(wú)人機(jī)能夠在未知或復(fù)雜環(huán)境中實(shí)時(shí)規(guī)劃軌跡，并保持對(duì)目標(biāo)的持續(xù)追蹤。然而，現(xiàn)有的運(yùn)動(dòng)規(guī)劃方法在處理動(dòng)態(tài)目標(biāo)追蹤任務(wù)時(shí)仍存在諸多問(wèn)題。

2、運(yùn)動(dòng)規(guī)劃由前端路徑規(guī)劃和后端軌跡優(yōu)化組成。目前，常見(jiàn)的無(wú)人機(jī)路徑規(guī)劃方法包括基于圖搜索的路徑搜索方法、隨機(jī)采樣方法和基于人工勢(shì)場(chǎng)的方法。例如，a*（a-star）算法能夠在復(fù)雜環(huán)境中找到最短路徑，但在復(fù)雜環(huán)境中計(jì)算開(kāi)銷較大，難以適應(yīng)實(shí)時(shí)動(dòng)態(tài)變化。跳點(diǎn)搜索（jump?point?search,?jps）雖然提高了搜索效率，但僅適用于離散網(wǎng)格空間，難以處理連續(xù)空間優(yōu)化問(wèn)題。此外，基于人工勢(shì)場(chǎng)的路徑規(guī)劃方法雖然在實(shí)時(shí)性方面具有一定優(yōu)勢(shì)，但其在復(fù)雜環(huán)境中容易陷入局部最優(yōu)，導(dǎo)致路徑振蕩或停滯。

3、在軌跡優(yōu)化方面，現(xiàn)有方法主要包括基于?b?樣條的軌跡優(yōu)化、基于安全飛行通道（sfc）的軌跡優(yōu)化，以及基于強(qiáng)化學(xué)習(xí)的軌跡優(yōu)化。b?樣條曲線能夠生成平滑連續(xù)的軌跡，但容易過(guò)擬合數(shù)據(jù)點(diǎn)，難以適應(yīng)復(fù)雜環(huán)境的全局優(yōu)化問(wèn)題?；诎踩w行通道的優(yōu)化方法能夠確保無(wú)人機(jī)飛行安全，但其計(jì)算量較大，且硬約束可能導(dǎo)致軌跡過(guò)于接近障礙物，增加碰撞風(fēng)險(xiǎn)。強(qiáng)化學(xué)習(xí)方法具備較強(qiáng)的適應(yīng)能力，但訓(xùn)練過(guò)程容易陷入局部最優(yōu)，且在實(shí)時(shí)性要求較高的任務(wù)中計(jì)算資源消耗較大。

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

1、本發(fā)明的目的在于提供一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法。

2、本發(fā)明提供了一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其包括以下步驟：

3、步驟一、獲取移動(dòng)物體周圍的環(huán)境信息并構(gòu)建柵格地圖，并在柵格地圖中標(biāo)記障礙物和移動(dòng)物體的當(dāng)前位置；

4、步驟二、定義當(dāng)前時(shí)刻的局部目標(biāo)點(diǎn)的位置：若跟蹤目標(biāo)在移動(dòng)物體的視野內(nèi)，則以跟蹤目標(biāo)的坐標(biāo)作為局部目標(biāo)點(diǎn)的位置；若跟蹤目標(biāo)不在移動(dòng)物體的視野內(nèi)，則根據(jù)移動(dòng)物體到跟蹤目標(biāo)脫離視野點(diǎn)的方向，獲取局部目標(biāo)點(diǎn)；采用路徑規(guī)劃算法獲取移動(dòng)物體到局部目標(biāo)點(diǎn)的最短路徑；

5、步驟三、采用樣條曲線對(duì)步驟二獲取的路徑進(jìn)行平滑優(yōu)化，根據(jù)平滑優(yōu)化后的優(yōu)化曲線獲取最終軌跡；

6、步驟四、控制移動(dòng)物體沿著最終軌跡移動(dòng)，直至到達(dá)局部目標(biāo)點(diǎn)的位置；

7、步驟五、重復(fù)步驟二至步驟四，完成移動(dòng)物體對(duì)跟蹤目標(biāo)的跟蹤。

8、作為優(yōu)選，所述的步驟二中，跟蹤目標(biāo)脫離視野下，局部目標(biāo)點(diǎn)的獲取方法為：

9、

10、其中，pcurrent(t)為移動(dòng)物體位置；為移動(dòng)物體到跟蹤目標(biāo)脫離視野點(diǎn)方向的單位向量；為權(quán)重因子。

11、作為優(yōu)選，所述的步驟三中，在柵格地圖中構(gòu)建高斯勢(shì)場(chǎng)，并在平滑優(yōu)化后根據(jù)障礙物的高斯勢(shì)場(chǎng)對(duì)優(yōu)化曲線進(jìn)行避障處理。

12、作為優(yōu)選，所述避障處理的過(guò)程如下：

13、檢測(cè)優(yōu)化曲線是否存在碰撞風(fēng)險(xiǎn)，若優(yōu)化曲線不存在碰撞風(fēng)險(xiǎn)，則以優(yōu)化曲線作為最終軌跡；若優(yōu)化曲線存在碰撞風(fēng)險(xiǎn)，則計(jì)算優(yōu)化曲線上所有存在碰撞風(fēng)險(xiǎn)的點(diǎn)的平均位置，并在障礙物的高斯勢(shì)場(chǎng)的最小勢(shì)能邊界中選取最接近平均位置的點(diǎn)作為臨近點(diǎn)；通過(guò)將平均點(diǎn)向臨近點(diǎn)方向移動(dòng)，獲得脫離碰撞風(fēng)險(xiǎn)的安全點(diǎn)；將安全點(diǎn)作為控制點(diǎn)添加到樣條曲線中重新優(yōu)化曲線，并將重新優(yōu)化后的曲線作為最終軌跡。

14、作為優(yōu)選，判斷優(yōu)化曲線是否存在碰撞風(fēng)險(xiǎn)的依據(jù)為：若存在優(yōu)化曲線上點(diǎn)的勢(shì)能高于障礙物的高斯勢(shì)場(chǎng)中的最小勢(shì)能，則優(yōu)化曲線存在碰撞風(fēng)險(xiǎn)。

15、作為優(yōu)選，所述的步驟三中，采用b樣條曲線對(duì)路徑進(jìn)行平滑優(yōu)化；以步驟二獲得的路徑中的路徑節(jié)點(diǎn)作為b樣條曲線的控制點(diǎn)。

16、作為優(yōu)選，所述的步驟二中，采用?a*算法獲取移動(dòng)物體到局部目標(biāo)點(diǎn)的路徑；所述a*算法中衡量從移動(dòng)物體到局部目標(biāo)點(diǎn)距離的啟發(fā)式函數(shù)的獲取方式如下：

17、

18、其中，表示從起始點(diǎn)到點(diǎn)的實(shí)際移動(dòng)代價(jià)，；為每步移動(dòng)的固定代價(jià)；表示從起始點(diǎn)到點(diǎn)的代價(jià)；為局部目標(biāo)點(diǎn)位置；pcurrent為移動(dòng)物體位置。

19、作為優(yōu)選，所述的移動(dòng)物體具體為無(wú)人機(jī)；控制無(wú)人機(jī)沿著最終軌跡移動(dòng)的方法如下：構(gòu)建無(wú)人機(jī)飛行控制剛體模型和其狀態(tài)方程；以預(yù)測(cè)軌跡與最終軌跡之間的距離誤差最小化作為代價(jià)函數(shù)對(duì)狀態(tài)方程進(jìn)行優(yōu)化，實(shí)現(xiàn)對(duì)無(wú)人機(jī)運(yùn)動(dòng)控制。

20、第二方面，本發(fā)明提供了一種計(jì)算機(jī)設(shè)備，其包括存儲(chǔ)器、處理器及存儲(chǔ)在存儲(chǔ)器上并可在處理器上運(yùn)行的計(jì)算機(jī)程序，所述存儲(chǔ)器存儲(chǔ)計(jì)算機(jī)程序；所述處理器執(zhí)行上述的運(yùn)動(dòng)規(guī)劃方法。

21、第三方面，本發(fā)明提供了一種可讀存儲(chǔ)介質(zhì)，其存儲(chǔ)有計(jì)算機(jī)程序；所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)用于實(shí)現(xiàn)上述的運(yùn)動(dòng)規(guī)劃方法。

22、本發(fā)明具有的有益效果是：

23、1、本發(fā)明通過(guò)在規(guī)劃過(guò)程中引入局部目標(biāo)點(diǎn)以優(yōu)化局部路徑規(guī)劃，解決了在跟蹤動(dòng)態(tài)目標(biāo)時(shí)存在部分時(shí)間段脫離視野以及a*算法在復(fù)雜環(huán)境中計(jì)算開(kāi)銷較大，難以適應(yīng)實(shí)時(shí)動(dòng)態(tài)變化的問(wèn)題，與全局路徑規(guī)劃相比，本發(fā)明能夠大幅減少搜索負(fù)擔(dān)；同時(shí)，本發(fā)明采用基于軟約束的高斯勢(shì)場(chǎng)?b?樣條優(yōu)化方法，使無(wú)人機(jī)的飛行軌跡更加平滑，避免急劇轉(zhuǎn)向帶來(lái)的飛行不穩(wěn)定性，相比傳統(tǒng)軌跡優(yōu)化方法，本發(fā)明避免了求解復(fù)雜的約束優(yōu)化問(wèn)題，使計(jì)算過(guò)程更加高效，通過(guò)設(shè)定動(dòng)態(tài)約束范圍，確保無(wú)人機(jī)在安全區(qū)域內(nèi)飛行。

24、2、本發(fā)明通過(guò)構(gòu)建高斯勢(shì)場(chǎng)結(jié)合局部動(dòng)態(tài)?a*算法，能夠快速生成符合安全的實(shí)時(shí)最優(yōu)軌跡，且在復(fù)雜環(huán)境下實(shí)現(xiàn)高效、平滑且安全的動(dòng)態(tài)目標(biāo)追蹤；同時(shí)，通過(guò)添加控制點(diǎn)的方式引導(dǎo)無(wú)人機(jī)避開(kāi)障礙物，降低碰撞風(fēng)險(xiǎn)，提高復(fù)雜環(huán)境中的目標(biāo)追蹤效率。

技術(shù)特征：

1.一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其特征在于：包括以下步驟：

2.根據(jù)權(quán)利要求1所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其特征在于：所述的步驟二中，跟蹤目標(biāo)脫離視野下，局部目標(biāo)點(diǎn)的獲取方法為：

3.根據(jù)權(quán)利要求1所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其特征在于：所述的步驟三中，在柵格地圖中構(gòu)建高斯勢(shì)場(chǎng)，并在平滑優(yōu)化后根據(jù)障礙物的高斯勢(shì)場(chǎng)對(duì)優(yōu)化曲線進(jìn)行避障處理。

4.根據(jù)權(quán)利要求3所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其特征在于：所述避障處理的過(guò)程如下：

5.根據(jù)權(quán)利要求4所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其特征在于：判斷優(yōu)化曲線是否存在碰撞風(fēng)險(xiǎn)的依據(jù)為：若存在優(yōu)化曲線上點(diǎn)的勢(shì)能高于障礙物的高斯勢(shì)場(chǎng)中的最小勢(shì)能，則優(yōu)化曲線存在碰撞風(fēng)險(xiǎn)。

6.根據(jù)權(quán)利要求1所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其特征在于：所述的步驟三中，采用b樣條曲線對(duì)路徑進(jìn)行平滑優(yōu)化；以步驟二獲得的路徑中的路徑節(jié)點(diǎn)作為b樣條曲線的控制點(diǎn)。

7.根據(jù)權(quán)利要求1所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其特征在于：所述的步驟二中，采用?a*算法獲取移動(dòng)物體到局部目標(biāo)點(diǎn)的路徑；所述a*算法中衡量從移動(dòng)物體到局部目標(biāo)點(diǎn)距離的啟發(fā)式函數(shù)的獲取方式如下：

8.根據(jù)權(quán)利要求1所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，其特征在于：所述的移動(dòng)物體具體為無(wú)人機(jī)；控制無(wú)人機(jī)沿著最終軌跡移動(dòng)的方法如下：構(gòu)建無(wú)人機(jī)飛行控制剛體模型和其狀態(tài)方程；以預(yù)測(cè)軌跡與最終軌跡之間的距離誤差最小化作為代價(jià)函數(shù)對(duì)狀態(tài)方程進(jìn)行優(yōu)化，實(shí)現(xiàn)對(duì)無(wú)人機(jī)運(yùn)動(dòng)控制。

9.一種計(jì)算機(jī)設(shè)備，包括存儲(chǔ)器、處理器及存儲(chǔ)在存儲(chǔ)器上并可在處理器上運(yùn)行的計(jì)算機(jī)程序，其特征在于：所述存儲(chǔ)器存儲(chǔ)計(jì)算機(jī)程序；所述處理器執(zhí)行如權(quán)利要求1-8中任意一項(xiàng)所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法。

10.一種可讀存儲(chǔ)介質(zhì)，存儲(chǔ)有計(jì)算機(jī)程序；其特征在于：所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)用于實(shí)現(xiàn)如權(quán)利要求1-8中任意一項(xiàng)所述的一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法。

技術(shù)總結(jié)
本發(fā)明公開(kāi)了一種基于動(dòng)態(tài)目標(biāo)追蹤的運(yùn)動(dòng)規(guī)劃方法，該運(yùn)動(dòng)規(guī)劃方法先進(jìn)行路徑規(guī)劃，通過(guò)定義當(dāng)前時(shí)刻的局部目標(biāo)點(diǎn)的位置，并采用路徑規(guī)劃算法獲取無(wú)人機(jī)到局部目標(biāo)點(diǎn)的最短路徑；然后采用B樣條曲線對(duì)最短路徑進(jìn)行平滑優(yōu)化，在基于障礙物的高斯勢(shì)場(chǎng)對(duì)平滑優(yōu)化后的優(yōu)化曲線進(jìn)行碰撞檢測(cè)后，獲取最終軌跡；最后，控制無(wú)人機(jī)沿著最終軌跡移動(dòng)，直至到達(dá)局部目標(biāo)點(diǎn)的位置。本發(fā)明通過(guò)在規(guī)劃過(guò)程中引入局部目標(biāo)點(diǎn)以優(yōu)化局部路徑規(guī)劃，大幅減少搜索負(fù)擔(dān)；同時(shí)，本發(fā)明采用高斯勢(shì)場(chǎng)B樣條優(yōu)化方法，使無(wú)人機(jī)的飛行軌跡更加平滑，且能夠在復(fù)雜環(huán)境下實(shí)現(xiàn)高效、平滑且安全的動(dòng)態(tài)目標(biāo)追蹤。

技術(shù)研發(fā)人員：楊帆,呂強(qiáng),張波濤
受保護(hù)的技術(shù)使用者：杭州電子科技大學(xué)
技術(shù)研發(fā)日：
技術(shù)公布日：2025/5/15