本發(fā)明屬于智能機(jī)器人協(xié)同控制，具體涉及一種基于輪足式平臺(tái)的多機(jī)器人區(qū)域覆蓋路徑規(guī)劃方法。

背景技術(shù)：

1、現(xiàn)有路徑規(guī)劃技術(shù)(如基于a*、dijkstra算法的圖搜索方法和rrt、prm等采樣算法)在處理大規(guī)模、復(fù)雜和動(dòng)態(tài)環(huán)境時(shí)存在一定的局限性。例如，圖搜索算法在高分辨率環(huán)境中容易產(chǎn)生計(jì)算量暴增的問(wèn)題，難以滿足實(shí)時(shí)規(guī)劃的需求，而采樣算法雖能較好地應(yīng)對(duì)高維空間，但在障礙密集區(qū)域中可能采樣不足，導(dǎo)致全局最優(yōu)解難以保證。同時(shí)，傳統(tǒng)方法在面對(duì)環(huán)境動(dòng)態(tài)變化、突發(fā)障礙和局部約束時(shí)，往往存在全局規(guī)劃與局部動(dòng)態(tài)調(diào)整之間銜接不暢的問(wèn)題，容易使得路徑平滑性和避障安全性受到影響，從而導(dǎo)致機(jī)器人偏離預(yù)定軌跡或形成冗長(zhǎng)路徑，降低整體系統(tǒng)的魯棒性和執(zhí)行效率。

2、輪足式機(jī)器人憑借其融合了輪式高效平穩(wěn)移動(dòng)和足式對(duì)復(fù)雜地形適應(yīng)性的優(yōu)點(diǎn)，能夠有效彌補(bǔ)現(xiàn)有路徑規(guī)劃技術(shù)的不足。借助自適應(yīng)反饋控制和分布式傳感技術(shù)，輪足機(jī)器人能夠?qū)崟r(shí)感知環(huán)境變化，靈活調(diào)整運(yùn)動(dòng)模式，實(shí)現(xiàn)從全局規(guī)劃到局部動(dòng)態(tài)重規(guī)劃的無(wú)縫銜接；同時(shí)，混合驅(qū)動(dòng)系統(tǒng)不僅保證了在平整路面上的高效行駛，還賦予了機(jī)器人在面對(duì)突發(fā)障礙和復(fù)雜地形時(shí)更高的抗干擾能力，從而提升了自主導(dǎo)航和任務(wù)執(zhí)行的整體性能和安全性。

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

1、為解決現(xiàn)有技術(shù)的不足，本發(fā)明提供一種基于輪足式平臺(tái)的多機(jī)器人區(qū)域覆蓋路徑規(guī)劃方法，本方法結(jié)合輪足機(jī)器人能夠適應(yīng)多種地形的特點(diǎn)，通過(guò)引入地形參數(shù)的多機(jī)器人任務(wù)預(yù)分配方法，將環(huán)境分割成多個(gè)子區(qū)域，每個(gè)機(jī)器人負(fù)責(zé)覆蓋其中的一個(gè)子區(qū)域，減少了路徑規(guī)劃算法的計(jì)算量和搜索空間；在子區(qū)域內(nèi)，通過(guò)一種改進(jìn)的stc算法實(shí)現(xiàn)從任意一個(gè)自由柵格出發(fā)，都能構(gòu)造一個(gè)覆蓋所有任務(wù)區(qū)域的路徑，在柵格地圖的條件下實(shí)現(xiàn)100％的路徑覆蓋；在實(shí)際任務(wù)階段，通過(guò)輪足機(jī)器人對(duì)環(huán)境的感知能力，實(shí)時(shí)反饋現(xiàn)實(shí)地形和實(shí)際狀態(tài)的變化，實(shí)時(shí)調(diào)整任務(wù)分配和路徑規(guī)劃。

2、為達(dá)到上述目的，本發(fā)明的技術(shù)方案如下：

3、一種基于輪足式平臺(tái)的多機(jī)器人區(qū)域覆蓋路徑規(guī)劃方法，包括以下步驟：

4、步驟一、讀取地圖中的障礙物信息與地形參數(shù)，將當(dāng)前地圖記錄為柵格地圖，根據(jù)輪足機(jī)器人運(yùn)動(dòng)特點(diǎn)和地形復(fù)雜程度將柵格地圖劃分為輪式覆蓋區(qū)域和足式覆蓋區(qū)域。每個(gè)機(jī)器人的起始位置作為其初始拓展位置。

5、步驟二、將地圖中的所有待覆蓋柵格分配給每個(gè)機(jī)器人作為其目標(biāo)覆蓋柵格，將每個(gè)機(jī)器人的目標(biāo)覆蓋柵格組成的區(qū)域定義為屬于該機(jī)器人的一個(gè)子區(qū)域，每個(gè)子區(qū)域是從選定的初始拓展位置開(kāi)始逐步以下文所說(shuō)明過(guò)程進(jìn)行拓展獲得，即機(jī)器人的數(shù)量等于子區(qū)域的數(shù)量。

6、定義當(dāng)前拓展位置為當(dāng)前時(shí)刻的前一次拓展時(shí)進(jìn)行拓展的柵格，用于執(zhí)行下一步區(qū)域拓展操作。當(dāng)前拓展位置是動(dòng)態(tài)更新的，隨著子區(qū)域拓展過(guò)程不斷遷移。

7、總地圖區(qū)域中的柵格分為三種類(lèi)型：自由柵格fg(free?grid)、已被分配策略分配過(guò)的柵格cg(coveragedgrid)和被障礙物占據(jù)的柵格og(obstacle?grid)。

8、當(dāng)前子區(qū)域的邊界狀態(tài)的定義如下：

9、遍歷當(dāng)前子區(qū)域中所有處于邊界的柵格，如果至少有一個(gè)柵格的周?chē)噜彇鸥裰写嬖谧杂蓶鸥?，則當(dāng)前子區(qū)域的邊界狀態(tài)為自由狀態(tài)；反之，則為阻塞狀態(tài)。柵格的周?chē)噜彇鸥袷侵笘鸥竦纳?、下、左、右四個(gè)方向的相鄰柵格；

10、根據(jù)當(dāng)前拓展位置的相鄰柵格類(lèi)型將當(dāng)前子區(qū)域定義如下三種狀態(tài)：

11、定義1：自由狀態(tài)：如果當(dāng)前拓展位置的周?chē)噜彇鸥裰辽俅嬖谝粋€(gè)自由柵格，那么當(dāng)前子區(qū)域就處于自由狀態(tài)。

12、定義2：阻塞狀態(tài)：如果當(dāng)前拓展位置的所有周?chē)噜彇鸥穸紴閏g柵格和og柵格，且邊界狀態(tài)也為阻塞狀態(tài)，則當(dāng)前子區(qū)域處于阻塞狀態(tài)。

13、定義3：偽阻塞狀態(tài)：如果當(dāng)前拓展位置的所有周?chē)噜彇鸥穸紴閏g柵格和og柵格，但邊界狀態(tài)為自由狀態(tài)，則當(dāng)前子區(qū)域處于偽阻塞狀態(tài)。

14、根據(jù)當(dāng)前子區(qū)域狀態(tài)不同，將選擇如下對(duì)應(yīng)的策略進(jìn)行分區(qū)拓展。

15、1)自由狀態(tài)策略：定義當(dāng)前拓展位置所有周?chē)噜彇鸥翊嬖诘淖杂蓶鸥駷楹蜻x柵格。若候選柵格只有一個(gè)，則選擇該柵格進(jìn)行拓展。若候選柵格有多個(gè)，則構(gòu)造一個(gè)獎(jiǎng)勵(lì)函數(shù)rf，計(jì)算每個(gè)候選柵格的獎(jiǎng)勵(lì)值，策略將選擇獎(jiǎng)勵(lì)值最高的候選柵格進(jìn)行拓展。

16、rf由距離獎(jiǎng)勵(lì)rfd和運(yùn)動(dòng)獎(jiǎng)勵(lì)rfs兩個(gè)部分組成。rfd表示候選柵格在當(dāng)前時(shí)刻狀態(tài)下，與其余所有機(jī)器人當(dāng)前子區(qū)域的當(dāng)前拓展位置的距離之和，rfd越高，表示機(jī)器人在接下來(lái)的區(qū)域拓展過(guò)程中進(jìn)入阻塞狀態(tài)的概率就越小。rfs表示從當(dāng)前柵格位置前往該候選柵格，進(jìn)行運(yùn)動(dòng)模式切換的收益，rfs越高，表示機(jī)器人完成覆蓋任務(wù)的能耗就越小。

17、候選柵格的集合表示為另外表示區(qū)域拓展過(guò)程中第t個(gè)時(shí)刻時(shí)第i個(gè)機(jī)器人處于的當(dāng)前拓展位置。f代表未分配。

18、rf的值由下式計(jì)算得出：

19、

20、其中k1和k2分別為rfd和rfs的系數(shù)，表示兩部分的權(quán)重。t表示柵格拓展階段的時(shí)間戳。其中表示第i個(gè)機(jī)器人在第t時(shí)刻的第m個(gè)候選柵格的獎(jiǎng)勵(lì)函數(shù)值。

21、rfd的值由下式計(jì)算得出：

22、

23、其中，‖‖表示求二者的歐氏距離，表示第i個(gè)機(jī)器人在第t個(gè)時(shí)刻的第m個(gè)候選柵格的位置，表示第i個(gè)機(jī)器人在第t個(gè)時(shí)刻的當(dāng)前拓展位置。

24、rfs的值由下式計(jì)算得出：

25、

26、其中，k3，k4，k5表示不同模式切換情況的收益值，根據(jù)機(jī)器人實(shí)際行動(dòng)能力進(jìn)行參數(shù)調(diào)整。

27、2)偽阻塞狀態(tài)策略：遍歷當(dāng)前子區(qū)域所有處于邊界的柵格，定義每個(gè)柵格所有周?chē)噜彇鸥翊嬖诘淖杂蓶鸥駷楹蜻x柵格。在第t個(gè)拓展時(shí)刻，第i個(gè)子區(qū)域有u∈n+個(gè)候選柵格能夠用于拓展。構(gòu)造一個(gè)拓展函數(shù)ef來(lái)評(píng)估候選柵格的拓展性能，策略將選擇ef值最低的候選柵格進(jìn)行拓展。

28、定義當(dāng)前拓展?fàn)顟B(tài)下，地圖中所有自由柵格組成的不規(guī)則圖形的質(zhì)心柵格為gfg，指示越靠近gfg的位置越不容易出現(xiàn)阻塞狀態(tài)。ef值定義為從偽阻塞狀態(tài)下所有候選柵格到gfg的可達(dá)路徑長(zhǎng)度，采用a*算法計(jì)算。若ef值越小，則代表這個(gè)候選柵格在接下來(lái)拓展過(guò)程中陷入阻塞狀態(tài)的可能性將更小。

29、為候選柵格的集合，第i個(gè)機(jī)器人在t拓展時(shí)刻的第u個(gè)候選柵格的ef值計(jì)算如下：

30、

31、其中，a*(·)代表基于a*算法計(jì)算可達(dá)路徑長(zhǎng)度，gfg(t)表示第t時(shí)刻任務(wù)區(qū)域中所有自由柵格組成的不規(guī)則圖形的質(zhì)心所在的柵格位置，表示處于偽阻塞狀態(tài)下的第i個(gè)機(jī)器人在第t時(shí)刻的第u個(gè)候選柵格位置。

32、另外，如果兩個(gè)候選柵格ef值相差很小，則采用自由狀態(tài)策略中運(yùn)動(dòng)獎(jiǎng)勵(lì)rfs的計(jì)算方法計(jì)算收益值，選擇收益值較高的候選柵格進(jìn)行拓展。

33、3)阻塞狀態(tài)策略：若當(dāng)前子區(qū)域進(jìn)入阻塞狀態(tài)，則暫停其拓展操作，等待其他分區(qū)完成分配。為避免最終分配結(jié)果存在嚴(yán)重不均衡的情況，將通過(guò)下一步驟進(jìn)行調(diào)整。

34、在步驟二中，以每個(gè)分區(qū)都完成一次分區(qū)拓展操作作為一次循環(huán)，將重復(fù)迭代此循環(huán)直到地圖中不存在自由柵格或所有分區(qū)都進(jìn)入阻塞狀態(tài)為止。

35、步驟三、分區(qū)拓展完成后，根據(jù)柵格數(shù)量與分區(qū)拓展后的子區(qū)域內(nèi)不同運(yùn)動(dòng)模態(tài)區(qū)域的分布情況，對(duì)分配結(jié)果進(jìn)行調(diào)整，以均衡每個(gè)機(jī)器人的能耗及任務(wù)量。具體過(guò)程為：

36、構(gòu)造一個(gè)任務(wù)量函數(shù)tf，包括柵格數(shù)量g和足式運(yùn)動(dòng)區(qū)域數(shù)量f兩個(gè)部分，根據(jù)機(jī)器人實(shí)際運(yùn)動(dòng)能力為他們賦予不同的權(quán)重為wg和wf。

37、整個(gè)區(qū)域被劃分為q個(gè)子區(qū)域，每個(gè)子區(qū)域的tf的值由下式計(jì)算得出：

38、tfq＝wggq+wffq

39、其中,q代表第q個(gè)子區(qū)域，wg和wf由機(jī)器人運(yùn)動(dòng)能力決定。

40、通過(guò)計(jì)算最優(yōu)均衡任務(wù)量tfopt，確定重分配各分區(qū)分配的數(shù)量目標(biāo)?？?cè)蝿?wù)量tftotal為：

41、

42、最優(yōu)的均衡任務(wù)量為：

43、

44、計(jì)算全局中所有分區(qū)結(jié)果中任務(wù)量最小的，將其確定為第一步重分配對(duì)象，該子區(qū)域從最后一個(gè)被分配的柵格開(kāi)始，繼續(xù)按照步驟二的方法拓展。找出當(dāng)前任務(wù)量最小的子區(qū)域j：

45、

46、此時(shí)將其余已被分配的柵格看作自由柵格以便當(dāng)前子區(qū)域進(jìn)行拓展。定義當(dāng)前拓展子區(qū)域j的柵格集為sj，待拓展的自由柵格集合為sfree，從sfree中選擇合適的柵格gk進(jìn)行分配：

47、

48、其中d(g,sj)表示柵格g到子區(qū)域sj的距離，策略將盡可能選擇最近的柵格進(jìn)行拓展。

49、在每一次重分配后通過(guò)判斷當(dāng)前拓展分區(qū)任務(wù)量是否達(dá)到最優(yōu)均衡任務(wù)量以及是否為當(dāng)前所有分區(qū)中任務(wù)量最小的來(lái)切換拓展分區(qū)，循環(huán)拓展過(guò)程直至所有分區(qū)都獲取到最優(yōu)分區(qū)柵格數(shù)。

50、步驟四、利用結(jié)合輪足式特點(diǎn)改進(jìn)的stc算法構(gòu)造子分區(qū)覆蓋路徑。

51、在子分區(qū)中，根據(jù)輪足機(jī)器人運(yùn)動(dòng)能力對(duì)以輪式區(qū)域節(jié)點(diǎn)為終點(diǎn)的邊賦權(quán)重衰減因子λ(0≤λ≤1)，對(duì)以足式區(qū)域節(jié)點(diǎn)為終點(diǎn)的邊賦權(quán)重增益因子采用prim算法建立最小生成樹(shù)，刪去柵格類(lèi)型為足式區(qū)域的葉子節(jié)點(diǎn)及其邊，先序或后序環(huán)繞最小生成樹(shù)，生成覆蓋路徑。再由被刪除節(jié)點(diǎn)通過(guò)a*算法向生成樹(shù)構(gòu)建最短路徑，標(biāo)記為最低優(yōu)先級(jí)路徑。實(shí)際任務(wù)階段時(shí)，機(jī)器人在完成所有覆蓋路徑之后，再前往被刪去的足式區(qū)域。

52、步驟五、在實(shí)際任務(wù)階段，輪足機(jī)器人通過(guò)慣性測(cè)量單元(imu)、激光雷達(dá)等傳感器感知實(shí)際的環(huán)境信息，通過(guò)機(jī)身具備的電源管理系統(tǒng)和關(guān)節(jié)驅(qū)動(dòng)報(bào)錯(cuò)機(jī)制確定自身的電量狀態(tài)、故障信息等狀態(tài)信息，對(duì)異常情況設(shè)計(jì)對(duì)應(yīng)的應(yīng)急處理機(jī)制。具體如下：

53、對(duì)機(jī)器人實(shí)際任務(wù)階段的自身信息建立健康評(píng)估函數(shù)

54、

55、其中，hfk(t)∈[0,1]表示機(jī)器人k在t時(shí)刻的健康度評(píng)估值，該值越大表示狀態(tài)越佳；μ，,τ為電量和故障項(xiàng)的調(diào)節(jié)系數(shù)，滿足μ+τ＝1；表示t時(shí)刻的剩余電量，為初始電量；v為非線性衰減因子(v≥1)，用于強(qiáng)化低電量狀態(tài)的影響；δa表示第a類(lèi)故障的嚴(yán)重程度權(quán)重，滿足∑δa＝1；fi∈{0,1}為故障指示函數(shù)，當(dāng)?shù)赼類(lèi)故障真實(shí)發(fā)生時(shí)為1。

56、設(shè)定健康度容許最小值hfmin，對(duì)實(shí)際任務(wù)中機(jī)器人建立淘汰機(jī)制，根據(jù)健康評(píng)估函數(shù)設(shè)計(jì)重分配階段各機(jī)器人的理想任務(wù)量為

57、

58、其中，tak表示第k個(gè)機(jī)器人的目標(biāo)任務(wù)量，ω＝{m|hfm＞hfmin}表示有效機(jī)器人的集合。

59、定義任務(wù)量差值δ＝tak-tfq。

60、異常情況一：當(dāng)出現(xiàn)識(shí)別到與初始地圖記錄的障礙物不符合的情況時(shí)，通過(guò)通信模塊匯報(bào)回總系統(tǒng)，系統(tǒng)更新柵格地圖，將當(dāng)前已完成覆蓋任務(wù)的區(qū)域視為初始子區(qū)域，當(dāng)前機(jī)器人的位置視為當(dāng)前拓展位置，重新執(zhí)行第二步驟進(jìn)行分區(qū)拓展，保存子區(qū)域結(jié)果；將當(dāng)前已完成覆蓋任務(wù)的區(qū)域視為障礙物，計(jì)算每個(gè)機(jī)器人的目標(biāo)任務(wù)量tak及其差值δ，優(yōu)先從δ最大的子區(qū)域開(kāi)始按照步驟三的方法進(jìn)行重分配；繼續(xù)執(zhí)行步驟四，完成本次異常處理。

61、異常情況二：當(dāng)出現(xiàn)地形情況與初始地圖記錄不符的情況時(shí)，通過(guò)通信模塊匯報(bào)系統(tǒng)，系統(tǒng)更新柵格地圖，按照與異常情況一相同的邏輯執(zhí)行步驟三和步驟四

62、異常情況三：某個(gè)機(jī)器人的hfk(t)≤hfmin，則將被其已覆蓋柵格包圍的自由柵格作為其最終覆蓋任務(wù)，將其已覆蓋區(qū)域及其最終覆蓋任務(wù)部分視為障礙物區(qū)域，按照異常情況一的處理方法執(zhí)行步驟二到步驟四。

63、本發(fā)明的有益效果在于通過(guò)地形參數(shù)驅(qū)動(dòng)的多機(jī)器人任務(wù)預(yù)分配與動(dòng)態(tài)調(diào)整機(jī)制，結(jié)合輪足式平臺(tái)的混合運(yùn)動(dòng)特性，顯著提升了復(fù)雜環(huán)境下的區(qū)域覆蓋效率?；诟倪M(jìn)stc算法的全覆蓋路徑生成方法，在保證100％覆蓋率的同時(shí)優(yōu)化了路徑平滑性與能耗分布，通過(guò)輪式/足式區(qū)域的權(quán)重因子動(dòng)態(tài)調(diào)整，有效減少冗余路徑長(zhǎng)度。系統(tǒng)具備強(qiáng)魯棒性，能夠?qū)崟r(shí)感知環(huán)境變化與機(jī)器人狀態(tài)，通過(guò)健康評(píng)估函數(shù)觸發(fā)任務(wù)重分配策略，確保在突發(fā)障礙、地形突變或機(jī)器人故障時(shí)仍能維持任務(wù)連續(xù)性。此外，輪足式平臺(tái)的多模態(tài)運(yùn)動(dòng)能力與路徑規(guī)劃深度協(xié)同，通過(guò)運(yùn)動(dòng)模式切換收益評(píng)估實(shí)現(xiàn)地形自適應(yīng)決策，兼顧了高效移動(dòng)與復(fù)雜地形適應(yīng)性的雙重需求。該方法不僅適用于大規(guī)模機(jī)器人集群協(xié)作，其基于柵格地圖的通用化設(shè)計(jì)還便于與現(xiàn)有導(dǎo)航系統(tǒng)集成，為災(zāi)害救援、區(qū)域巡檢等場(chǎng)景提供了高效可靠的解決方案。