本發(fā)明屬于移動機(jī)器人動態(tài)避障，具體涉及一種基于融合卡爾曼濾波和改進(jìn)dwa算法的動態(tài)避障方法。

背景技術(shù)：

1、在移動機(jī)器人領(lǐng)域，動態(tài)避障是指移動機(jī)器人在不斷變化的環(huán)境中，實時調(diào)整自身路徑以避開障礙物并安全抵達(dá)目標(biāo)位置的過程。隨著技術(shù)的快速發(fā)展，移動機(jī)器人被廣泛應(yīng)用于服務(wù)、農(nóng)業(yè)、醫(yī)療等各領(lǐng)域。在實際應(yīng)用中，移動機(jī)器人需要面對行人、機(jī)器人及其他移動物體等動態(tài)障礙物的干擾，這些障礙物可能導(dǎo)致移動機(jī)器人行駛速度降低、偏離預(yù)定軌跡，甚至中斷任務(wù)。因此，動態(tài)避障的研究對于確保移動機(jī)器人在復(fù)雜環(huán)境中安全、高效地完成任務(wù)具有重要意義。

2、動態(tài)窗口法(dynamic?window?approach，簡稱dwa)是一種用于避障的局部路徑規(guī)劃算法，在考慮速度和加速度限制的前提下，生成可能的速度采樣點，并通過評價函數(shù)選擇最優(yōu)軌跡。

3、dwa算法在解決動態(tài)避障問題時，缺少對動態(tài)障礙物的軌跡預(yù)測，往往將動態(tài)障礙物視為瞬時靜態(tài)障礙物來處理，忽略了障礙物的速度大小和運(yùn)動方向，導(dǎo)致機(jī)器人避障反應(yīng)遲緩、路徑規(guī)劃效率低。

4、此外，范(doi:10.1109/jsyst.2023.3274187.)等人針對dwa算法中局部最小值的缺陷，提出了一種幫助機(jī)器人逃脫u形陷阱的低速轉(zhuǎn)向策略。

5、然而，這些技術(shù)主要是解決由靜態(tài)障礙物構(gòu)成的局部最優(yōu)陷阱，缺少對動態(tài)障礙物所構(gòu)成的局部最優(yōu)陷阱，即動態(tài)障礙物與移動機(jī)器人易陷入持續(xù)性平行狀態(tài)的處理。

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

1、為了克服以上現(xiàn)有技術(shù)存在的問題，本發(fā)明提供一種基于融合卡爾曼濾波和改進(jìn)dwa算法的動態(tài)避障方法，該方法提高了機(jī)器人在復(fù)雜環(huán)境中的避障能力、路徑規(guī)劃精度以及避免陷入局部最優(yōu)陷阱，提高了全局規(guī)劃能力。

2、為了實現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案是：

3、一種基于融合卡爾曼濾波和改進(jìn)dwa算法的動態(tài)避障方法，包括以下步驟；

4、步驟1：利用激光雷達(dá)獲取移動機(jī)器人的起始位置、目標(biāo)位置以及動、靜態(tài)障礙物信息；

5、步驟2：根據(jù)激光雷達(dá)獲取的動態(tài)障礙物位置、速度觀測信息，使用卡爾曼濾波算法進(jìn)行障礙物軌跡預(yù)測，獲得障礙物位置、速度最優(yōu)估計值；

6、步驟3：將上述獲得的動、靜障礙物位置、速度最優(yōu)估計值加入dwa算法中，增加其對動態(tài)軌跡的預(yù)測功能，大大提高了避障的準(zhǔn)確性以及縮短了避障時間。

7、步驟4：使用改進(jìn)后的評價函數(shù)對dwa算法速度窗口中的速度對進(jìn)行評分，選取評分最高的速度對直至找到目標(biāo)點，解決易陷入動態(tài)局部最優(yōu)問題。

8、所述步驟1具體包括：

9、利用激光雷達(dá)獲取移動機(jī)器人的起始位置、目標(biāo)位置以及動、靜態(tài)障礙物信息；其中，起始坐標(biāo)為(xq,yq)，目標(biāo)坐標(biāo)為(xz,yz)，障礙物坐標(biāo)為(obxi,obyi)，i為障礙物個數(shù)。

10、所述步驟2具體包括：

11、記錄激光雷達(dá)實時采集的動態(tài)障礙物位置、速度；

12、根據(jù)空間狀態(tài)方程以及觀測方程建立系統(tǒng)模型；

13、空間狀態(tài)方程：xk＝axk-1+buk-1+wk-1

14、觀測方程：zk＝hxk+vk

15、其中：xk、xk-1為k、k-1時刻系統(tǒng)的真實狀態(tài)，zk為k時刻觀測到的狀態(tài)，a為狀態(tài)轉(zhuǎn)移矩陣，b為控制輸入矩陣，h為狀態(tài)觀測矩陣，wk-1為系統(tǒng)狀態(tài)的隨機(jī)干擾噪聲，vk為觀測噪聲；

16、利用卡爾曼濾波算法進(jìn)行預(yù)測與矯正，輸出k時刻的系統(tǒng)狀態(tài)最優(yōu)估計值以及k+dt時刻的系統(tǒng)狀態(tài)預(yù)測值；

17、所述卡爾曼濾波算法進(jìn)行預(yù)測與矯正，過程如下：

18、預(yù)測過程：基于k-1時刻的系統(tǒng)狀態(tài)最優(yōu)估計值，通過下述方程，對k時刻系統(tǒng)的狀態(tài)進(jìn)行預(yù)測，并得出此時刻下對應(yīng)的系統(tǒng)預(yù)估誤差協(xié)方差矩陣

19、

20、其中：與分別表示k時刻的系統(tǒng)狀態(tài)預(yù)測值和k-1時刻的系統(tǒng)狀態(tài)的最優(yōu)估計值，表示k時刻對應(yīng)的系統(tǒng)預(yù)估誤差協(xié)方差矩陣，q表示系統(tǒng)工作過程的協(xié)方差矩陣；

21、矯正過程：通過卡爾曼濾波增益矩陣kk結(jié)合k時刻的系統(tǒng)狀態(tài)預(yù)測值以及k時刻傳感器觀測到的狀態(tài)zk得出k時刻系統(tǒng)狀態(tài)最優(yōu)估計值與k時刻濾波誤差協(xié)方差的最優(yōu)估計值pk；

22、

23、其中：r表示測量噪聲協(xié)方差矩陣，i為單位矩陣。

24、所述步驟3具體包括：

25、(1)選取窗口：根據(jù)速度約束、加速度約束、安全約束得到動態(tài)窗口集合vm＝vn∩va∩vd；

26、(2)模擬預(yù)測軌跡：根據(jù)窗口中的速度進(jìn)行采樣，模擬預(yù)測軌跡；

27、(3)獲取動態(tài)障礙物預(yù)測信息：定義速度窗口預(yù)測時間t，利用步驟2獲取該預(yù)測時間內(nèi)動態(tài)障礙物的位置、速度最優(yōu)估計值；

28、(4)融合信息：通過軌跡末端的狀態(tài)計算航向角偏差、距障礙物的最小距離、制動距離、速度大??；其中，距障礙物的最小距離、制動距離以移動機(jī)器人軌跡末端狀態(tài)與(3)中障礙物t秒后的最優(yōu)估計值進(jìn)行計算，在dwa算法中融入了動態(tài)障礙物的軌跡預(yù)測信息。

29、所述步驟(1)具體為：

30、速度約束：

31、vn＝{v∈[vmin,vmax],ω∈[ωmin,ωmax]}

32、其中vmin和vmax表示機(jī)器人最小和最大線速度，wmin和wmax表示機(jī)器人最小和最大角速度；

33、加速度約束：在一個動態(tài)窗口周期內(nèi)能夠達(dá)到的運(yùn)動速度集合定義為：

34、

35、其中，vr和ωr分別為機(jī)器人當(dāng)前時刻的線速度與角速度，和分別為最大線加速度與最大角加速度，主要由電機(jī)性能決定；δt為采樣時間間隔；

36、安全約束：定義滿足機(jī)器人運(yùn)動安全的速度空間為：

37、

38、其中，dist(v,ω)為當(dāng)前運(yùn)動軌跡上距離最近障礙物的距離；

39、依據(jù)上述三方面約束條件，取三種速度約束的交集得到機(jī)器人最合適的動態(tài)窗口vm。

40、所述步驟4具體包括：

41、在選擇最優(yōu)軌跡時，卡爾曼濾波融合dwa算法考慮了機(jī)器人的運(yùn)動約束以及動態(tài)障礙物的影響，以確保機(jī)器人能夠安全到達(dá)目標(biāo)點；此外還設(shè)計了方向角評價函數(shù)，用來保證路徑質(zhì)量，避免陷入動態(tài)局部最優(yōu)陷阱，改進(jìn)后的評價函數(shù)如下：

42、g(v,ω)＝σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)+δ·bearing(v,ω))

43、式中，heading(v,ω)為航向角評價子函數(shù)，移動機(jī)器人預(yù)測軌跡與目標(biāo)點之間的偏角，偏角越小得分越高；dist(v,ω)為障礙物距離評價子函數(shù)，當(dāng)前速度下對應(yīng)預(yù)測軌跡與障礙物之間的最小距離，距離越遠(yuǎn)評分越高；velocity(v,ω)為速度評價子函數(shù)，在確保安全避障的同時，速度越快得分越高；bearing(v,ω)為方向角評價子函數(shù)，移動機(jī)器人預(yù)測軌跡與最近動態(tài)障礙物之間的角度差，角度差越大得分越高；

44、α、β、γ、δ為評價函數(shù)的權(quán)重系數(shù)，σ表示對評價函數(shù)進(jìn)行歸一化處理。

45、方向角評價子函數(shù)的公式為δθ＝|θrobot-θobstacle|，其中θrobot是移動機(jī)器人各預(yù)測軌跡的角度，θobstacle是距離移動機(jī)器人最近的動態(tài)障礙物當(dāng)前角度，具體使用步驟為：

46、分析并記錄所出現(xiàn)的各個障礙物航向角，并計算各障礙物與移動機(jī)器人之間的距離；

47、找出距離移動機(jī)器人最近的障礙物以及它所對應(yīng)的航向角；

48、計算該障礙物δt＝0.1s內(nèi)的距離變化，若距離變化為0，判斷其為靜態(tài)障礙物，不使用該評價子函數(shù)；若距離不為0，則判斷其為動態(tài)障礙物，使用該評價子函數(shù)。

49、經(jīng)上述步驟，可以得出一條得分最高、考慮動態(tài)障礙物運(yùn)動趨勢、順利避開障礙物、避免陷入動態(tài)局部最優(yōu)的路徑，使得移動機(jī)器人完成避障路徑的最優(yōu)規(guī)劃。

50、本發(fā)明的有益效果：

51、本發(fā)明首先將卡爾曼濾波融入dwa算法中，使用卡爾曼濾波預(yù)估動態(tài)障礙物的速度、位置，使dwa算法在規(guī)劃避障軌跡時，考慮更全面；之后對融合算法評價函數(shù)進(jìn)行優(yōu)化，通過添加新的評價子函數(shù)來解決易陷入動態(tài)局部最優(yōu)解問題。

52、將卡爾曼濾波算法與dwa算法相結(jié)合，通過把卡爾曼濾波算法所預(yù)測到的動態(tài)障礙物未來運(yùn)動信息添加至dwa評價函數(shù)中，在保留dwa實時性的基礎(chǔ)上，增強(qiáng)對動態(tài)障礙物的預(yù)測能力，提高了移動機(jī)器人避障的準(zhǔn)確性和可靠性。對dwa算法進(jìn)行改進(jìn)，添加新的方向角評價子函數(shù)，實時考慮移動機(jī)器人各預(yù)測軌跡與障礙物之間的角度差，角度差越大得分越高，通過角度差的引入來避免陷入持續(xù)平行的動態(tài)局部最優(yōu)問題，提高了移動機(jī)器人避障的及時性和全局性。