本發(fā)明屬于路徑規(guī)劃，具體涉及一種基于動態(tài)目標追蹤的運動規(guī)劃方法。
背景技術(shù)：
1、隨著無人機技術(shù)的發(fā)展，無人機在軍事偵察、災害搜救、環(huán)境監(jiān)測等領(lǐng)域的應用越來越廣泛。動態(tài)目標追蹤是無人機自主導航的一項關(guān)鍵技術(shù)，要求無人機能夠在未知或復雜環(huán)境中實時規(guī)劃軌跡，并保持對目標的持續(xù)追蹤。然而，現(xiàn)有的運動規(guī)劃方法在處理動態(tài)目標追蹤任務時仍存在諸多問題。
2、運動規(guī)劃由前端路徑規(guī)劃和后端軌跡優(yōu)化組成。目前，常見的無人機路徑規(guī)劃方法包括基于圖搜索的路徑搜索方法、隨機采樣方法和基于人工勢場的方法。例如，a*（a-star）算法能夠在復雜環(huán)境中找到最短路徑，但在復雜環(huán)境中計算開銷較大，難以適應實時動態(tài)變化。跳點搜索（jump?point?search,?jps）雖然提高了搜索效率，但僅適用于離散網(wǎng)格空間，難以處理連續(xù)空間優(yōu)化問題。此外，基于人工勢場的路徑規(guī)劃方法雖然在實時性方面具有一定優(yōu)勢，但其在復雜環(huán)境中容易陷入局部最優(yōu)，導致路徑振蕩或停滯。
3、在軌跡優(yōu)化方面，現(xiàn)有方法主要包括基于?b?樣條的軌跡優(yōu)化、基于安全飛行通道（sfc）的軌跡優(yōu)化，以及基于強化學習的軌跡優(yōu)化。b?樣條曲線能夠生成平滑連續(xù)的軌跡，但容易過擬合數(shù)據(jù)點，難以適應復雜環(huán)境的全局優(yōu)化問題?；诎踩w行通道的優(yōu)化方法能夠確保無人機飛行安全，但其計算量較大，且硬約束可能導致軌跡過于接近障礙物，增加碰撞風險。強化學習方法具備較強的適應能力，但訓練過程容易陷入局部最優(yōu)，且在實時性要求較高的任務中計算資源消耗較大。
技術(shù)實現(xiàn)思路
1、本發(fā)明的目的在于提供一種基于動態(tài)目標追蹤的運動規(guī)劃方法。
2、本發(fā)明提供了一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其包括以下步驟：
3、步驟一、獲取移動物體周圍的環(huán)境信息并構(gòu)建柵格地圖，并在柵格地圖中標記障礙物和移動物體的當前位置；
4、步驟二、定義當前時刻的局部目標點的位置：若跟蹤目標在移動物體的視野內(nèi)，則以跟蹤目標的坐標作為局部目標點的位置；若跟蹤目標不在移動物體的視野內(nèi)，則根據(jù)移動物體到跟蹤目標脫離視野點的方向，獲取局部目標點；采用路徑規(guī)劃算法獲取移動物體到局部目標點的最短路徑；
5、步驟三、采用樣條曲線對步驟二獲取的路徑進行平滑優(yōu)化，根據(jù)平滑優(yōu)化后的優(yōu)化曲線獲取最終軌跡；
6、步驟四、控制移動物體沿著最終軌跡移動，直至到達局部目標點的位置；
7、步驟五、重復步驟二至步驟四，完成移動物體對跟蹤目標的跟蹤。
8、作為優(yōu)選，所述的步驟二中，跟蹤目標脫離視野下，局部目標點的獲取方法為：
9、
10、其中，pcurrent(t)為移動物體位置；為移動物體到跟蹤目標脫離視野點方向的單位向量；為權(quán)重因子。
11、作為優(yōu)選，所述的步驟三中，在柵格地圖中構(gòu)建高斯勢場，并在平滑優(yōu)化后根據(jù)障礙物的高斯勢場對優(yōu)化曲線進行避障處理。
12、作為優(yōu)選，所述避障處理的過程如下：
13、檢測優(yōu)化曲線是否存在碰撞風險，若優(yōu)化曲線不存在碰撞風險，則以優(yōu)化曲線作為最終軌跡；若優(yōu)化曲線存在碰撞風險，則計算優(yōu)化曲線上所有存在碰撞風險的點的平均位置，并在障礙物的高斯勢場的最小勢能邊界中選取最接近平均位置的點作為臨近點；通過將平均點向臨近點方向移動，獲得脫離碰撞風險的安全點；將安全點作為控制點添加到樣條曲線中重新優(yōu)化曲線，并將重新優(yōu)化后的曲線作為最終軌跡。
14、作為優(yōu)選，判斷優(yōu)化曲線是否存在碰撞風險的依據(jù)為：若存在優(yōu)化曲線上點的勢能高于障礙物的高斯勢場中的最小勢能，則優(yōu)化曲線存在碰撞風險。
15、作為優(yōu)選，所述的步驟三中，采用b樣條曲線對路徑進行平滑優(yōu)化；以步驟二獲得的路徑中的路徑節(jié)點作為b樣條曲線的控制點。
16、作為優(yōu)選，所述的步驟二中，采用?a*算法獲取移動物體到局部目標點的路徑；所述a*算法中衡量從移動物體到局部目標點距離的啟發(fā)式函數(shù)的獲取方式如下：
17、
18、其中，表示從起始點到點的實際移動代價，；為每步移動的固定代價；表示從起始點到點的代價；為局部目標點位置；pcurrent為移動物體位置。
19、作為優(yōu)選，所述的移動物體具體為無人機；控制無人機沿著最終軌跡移動的方法如下：構(gòu)建無人機飛行控制剛體模型和其狀態(tài)方程；以預測軌跡與最終軌跡之間的距離誤差最小化作為代價函數(shù)對狀態(tài)方程進行優(yōu)化，實現(xiàn)對無人機運動控制。
20、第二方面，本發(fā)明提供了一種計算機設(shè)備，其包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機程序，所述存儲器存儲計算機程序；所述處理器執(zhí)行上述的運動規(guī)劃方法。
21、第三方面，本發(fā)明提供了一種可讀存儲介質(zhì)，其存儲有計算機程序；所述計算機程序被處理器執(zhí)行時用于實現(xiàn)上述的運動規(guī)劃方法。
22、本發(fā)明具有的有益效果是：
23、1、本發(fā)明通過在規(guī)劃過程中引入局部目標點以優(yōu)化局部路徑規(guī)劃，解決了在跟蹤動態(tài)目標時存在部分時間段脫離視野以及a*算法在復雜環(huán)境中計算開銷較大，難以適應實時動態(tài)變化的問題，與全局路徑規(guī)劃相比，本發(fā)明能夠大幅減少搜索負擔；同時，本發(fā)明采用基于軟約束的高斯勢場?b?樣條優(yōu)化方法，使無人機的飛行軌跡更加平滑，避免急劇轉(zhuǎn)向帶來的飛行不穩(wěn)定性，相比傳統(tǒng)軌跡優(yōu)化方法，本發(fā)明避免了求解復雜的約束優(yōu)化問題，使計算過程更加高效，通過設(shè)定動態(tài)約束范圍，確保無人機在安全區(qū)域內(nèi)飛行。
24、2、本發(fā)明通過構(gòu)建高斯勢場結(jié)合局部動態(tài)?a*算法，能夠快速生成符合安全的實時最優(yōu)軌跡，且在復雜環(huán)境下實現(xiàn)高效、平滑且安全的動態(tài)目標追蹤；同時，通過添加控制點的方式引導無人機避開障礙物，降低碰撞風險，提高復雜環(huán)境中的目標追蹤效率。
1.一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其特征在于：包括以下步驟：
2.根據(jù)權(quán)利要求1所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其特征在于：所述的步驟二中，跟蹤目標脫離視野下，局部目標點的獲取方法為：
3.根據(jù)權(quán)利要求1所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其特征在于：所述的步驟三中，在柵格地圖中構(gòu)建高斯勢場，并在平滑優(yōu)化后根據(jù)障礙物的高斯勢場對優(yōu)化曲線進行避障處理。
4.根據(jù)權(quán)利要求3所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其特征在于：所述避障處理的過程如下：
5.根據(jù)權(quán)利要求4所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其特征在于：判斷優(yōu)化曲線是否存在碰撞風險的依據(jù)為：若存在優(yōu)化曲線上點的勢能高于障礙物的高斯勢場中的最小勢能，則優(yōu)化曲線存在碰撞風險。
6.根據(jù)權(quán)利要求1所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其特征在于：所述的步驟三中，采用b樣條曲線對路徑進行平滑優(yōu)化；以步驟二獲得的路徑中的路徑節(jié)點作為b樣條曲線的控制點。
7.根據(jù)權(quán)利要求1所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其特征在于：所述的步驟二中，采用?a*算法獲取移動物體到局部目標點的路徑；所述a*算法中衡量從移動物體到局部目標點距離的啟發(fā)式函數(shù)的獲取方式如下：
8.根據(jù)權(quán)利要求1所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法，其特征在于：所述的移動物體具體為無人機；控制無人機沿著最終軌跡移動的方法如下：構(gòu)建無人機飛行控制剛體模型和其狀態(tài)方程；以預測軌跡與最終軌跡之間的距離誤差最小化作為代價函數(shù)對狀態(tài)方程進行優(yōu)化，實現(xiàn)對無人機運動控制。
9.一種計算機設(shè)備，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機程序，其特征在于：所述存儲器存儲計算機程序；所述處理器執(zhí)行如權(quán)利要求1-8中任意一項所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法。
10.一種可讀存儲介質(zhì)，存儲有計算機程序；其特征在于：所述計算機程序被處理器執(zhí)行時用于實現(xiàn)如權(quán)利要求1-8中任意一項所述的一種基于動態(tài)目標追蹤的運動規(guī)劃方法。