[發(fā)明專利]全覆蓋應(yīng)用下異構(gòu)多機(jī)器人系統(tǒng)的任務(wù)規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202011401042.8
• 申請日: 2020-12-02
• 公開（公告）號: CN112462783B
• 公開（公告）日: 2021-07-23
• 發(fā)明（設(shè)計）人: 楊文婧;李林;藍(lán)龍;楊紹武;徐利洋;黃達(dá);史殿習(xí);楊思寧;崔玉寧;劉哲
• 申請（專利權(quán)）人: 中國人民解放軍國防科技大學(xué)
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 湖南企企衛(wèi)知識產(chǎn)權(quán)代理有限公司 43257
• 代理人: 任合明

1.一種全覆蓋應(yīng)用下異構(gòu)多機(jī)器人系統(tǒng)的任務(wù)規(guī)劃方法，其特征在于包括以下步驟：

第一步，構(gòu)建全覆蓋應(yīng)用下的異構(gòu)多機(jī)器人任務(wù)規(guī)劃系統(tǒng)，異構(gòu)多機(jī)器人任務(wù)規(guī)劃系統(tǒng)由服務(wù)端、客戶端及機(jī)器人終端組成；服務(wù)端是一臺服務(wù)器，其上安裝有異構(gòu)多機(jī)器人任務(wù)分配模塊、異構(gòu)多機(jī)器人路徑規(guī)劃模塊；

客戶端是一臺服務(wù)器或PC機(jī)或移動終端，其上存放有統(tǒng)一機(jī)器人定義格式文件即URDF文件，客戶端上有探測傳感器，通過探測傳感器獲取目標(biāo)區(qū)域的信息，計算目標(biāo)區(qū)域的待覆蓋總面積Area并確定策略選擇參數(shù)Stra，將N臺異構(gòu)機(jī)器人的URDF文件、目標(biāo)區(qū)域的信息、目標(biāo)區(qū)域的待覆蓋面積Area發(fā)送給異構(gòu)多機(jī)器人任務(wù)分配模塊，將Stra發(fā)送給異構(gòu)多機(jī)器人路徑規(guī)劃模塊；所述目標(biāo)區(qū)域的信息包括目標(biāo)區(qū)域的面積、障礙物、邊界；

異構(gòu)多機(jī)器人任務(wù)分配模塊與客戶端和異構(gòu)多機(jī)器人路徑規(guī)劃模塊相連，由機(jī)器人選擇模塊及環(huán)境建模模塊組成；機(jī)器人選擇模塊與客戶端及環(huán)境建模模塊相連，從客戶端接收URDF文件及目標(biāo)區(qū)域的待覆蓋總面積Area，將覆蓋任務(wù)與機(jī)器人能力進(jìn)行匹配，得到分配給覆蓋任務(wù)的最優(yōu)機(jī)器人集合C及其覆蓋面積集合CA，將C和CA發(fā)送給環(huán)境建模模塊，C中含K個機(jī)器人，1≤K≤N；環(huán)境建模模塊與客戶端、機(jī)器人選擇模塊及異構(gòu)多機(jī)器人路徑規(guī)劃模塊相連，從客戶端獲取Area、障礙物、邊界信息，對目標(biāo)區(qū)域進(jìn)行網(wǎng)格建模，得到建模后的二維網(wǎng)格矩陣D，從機(jī)器人選擇模塊獲取C及CA，結(jié)合建模得到的二維網(wǎng)格矩陣D，求得機(jī)器人集合C內(nèi)各機(jī)器人的待覆蓋網(wǎng)格數(shù)量集合G、起始網(wǎng)格坐標(biāo)集合S、網(wǎng)格數(shù)量比值集合H，將C、D、G、S、H發(fā)送給異構(gòu)多機(jī)器人路徑規(guī)劃模塊；

異構(gòu)多機(jī)器人路徑規(guī)劃模塊與異構(gòu)多機(jī)器人任務(wù)分配模塊、客戶端及N個機(jī)器人終端相連，由區(qū)域劃分模塊及并行路徑規(guī)劃模塊兩個子模塊組成；區(qū)域劃分模塊與客戶端、環(huán)境建模模塊及并行路徑規(guī)劃模塊相連，由初始劃分模塊及二次劃分模塊組成；初始劃分模塊與客戶端、環(huán)境建模模塊及二次劃分模塊相連，從環(huán)境建模模塊獲取目標(biāo)區(qū)域網(wǎng)格建模后的二維網(wǎng)格矩陣D、最優(yōu)分配機(jī)器人集合C及其起始網(wǎng)格集合S、待覆蓋網(wǎng)格數(shù)量G、網(wǎng)格數(shù)量比值集合H，從客戶端獲取策略選擇參數(shù)Stra，對目標(biāo)區(qū)域進(jìn)行初始劃分，得到初始子區(qū)域集合P，將P發(fā)送給二次劃分模塊；二次劃分模塊與初始劃分模塊、并行路徑規(guī)劃相連，對從初始劃分模塊接收的P進(jìn)行二次劃分，得到精確的區(qū)域劃分結(jié)果P’，將P’發(fā)送給并行路徑規(guī)劃模塊；并行路徑規(guī)劃模塊與二次劃分模塊及N個機(jī)器人終端相連，從二次劃分模塊獲取C中K臺機(jī)器人的精確子區(qū)域劃分結(jié)果P’，然后在P’中的K個待覆蓋子區(qū)域中并行進(jìn)行規(guī)劃路徑，得到K個機(jī)器人的覆蓋路徑，并將K個機(jī)器人的覆蓋路徑分別輸出到與C對應(yīng)的K臺機(jī)器人終端；

N個機(jī)器人終端是與URDF文件相對應(yīng)的N臺機(jī)器人，其中的K臺機(jī)器人從并行路徑規(guī)劃模塊接收對應(yīng)的覆蓋路徑，并行按照對應(yīng)的覆蓋路徑運(yùn)動，生成K條運(yùn)動軌跡；

第二步，客戶端的探測傳感器采集目標(biāo)區(qū)域的信息，根據(jù)目標(biāo)區(qū)域的信息計算目標(biāo)區(qū)域的待覆蓋總面積Area，將N臺異構(gòu)機(jī)器人的URDF文件、目標(biāo)區(qū)域的待覆蓋面積Area發(fā)送給機(jī)器人選擇模塊，將目標(biāo)區(qū)域的信息發(fā)送給環(huán)境建模模塊；

第三步，客戶端確定策略選擇參數(shù)Stra為1或2，將Stra發(fā)送給初始劃分模塊；

第四步，機(jī)器人選擇模塊從客戶端接收N臺異構(gòu)機(jī)器人的URDF文件和目標(biāo)區(qū)域的待覆蓋面積Area，根據(jù)URDF文件定義一個異構(gòu)多機(jī)器人集合A＝{a₁，a₂，...，a_n，...，a_N}，N為正整數(shù)，1≤n≤N，a_n為A中的第n臺機(jī)器人的序號；

第五步，機(jī)器人選擇模塊根據(jù)A中N臺機(jī)器人所對應(yīng)的URDF文件及目標(biāo)區(qū)域的待覆蓋面積Area，從A中分配最優(yōu)的K臺機(jī)器人給覆蓋任務(wù)，這K臺機(jī)器人組成最優(yōu)機(jī)器人集合C＝{c₁，c₂，...，c_k，...，c_K}，K為正整數(shù)，1≤K≤N，1≤k≤K；方法為：

5.1構(gòu)建異構(gòu)機(jī)器人能力模型，使其能從A中篩選出具備覆蓋能力的機(jī)器人集合，具體步驟如下：

5.1.1構(gòu)建機(jī)器人本體，本體是指一種形式化的、對共享概念體系的明確而又詳細(xì)的說明；

5.1.2根據(jù)機(jī)器人的URDF文件，為A中的N臺機(jī)器人進(jìn)行本體實(shí)例化，得到N個異構(gòu)機(jī)器人本體實(shí)例，對于第n臺機(jī)器人a_n，此步即根據(jù)a_n的URDF文件采用本體這個數(shù)據(jù)結(jié)構(gòu)對a_n進(jìn)行描述，得到a_n的本體實(shí)例；

5.1.3定義機(jī)器人覆蓋任務(wù)描述，該任務(wù)描述包含了機(jī)器人執(zhí)行一個覆蓋任務(wù)時所必備的R個技能，用技能集合Skill＝{Skill₁，Skill₂，...，Skill_r，...，Skill_R}表示，R為正整數(shù)，1≤r≤R；

5.1.4根據(jù)執(zhí)行覆蓋任務(wù)的機(jī)器人必須具備覆蓋任務(wù)描述中所有技能的原則，對A中N個機(jī)器人的本體實(shí)例進(jìn)行推理，從A中篩選出具備覆蓋能力的M臺機(jī)器人，組成具備覆蓋能力的機(jī)器人集合B＝{b₁，b₂，...，b_m，...，b_M}，M為正整數(shù)，1≤M≤N，1≤m≤M；

5.2制定基于機(jī)器人覆蓋面積的數(shù)學(xué)模型，使得從機(jī)器人集合B中得到分配給當(dāng)前覆蓋任務(wù)的最優(yōu)機(jī)器人集合，方法如下：

5.2.1構(gòu)建基于機(jī)器人覆蓋面積的數(shù)學(xué)模型，如公式(1)、(2)、(3)所示：

(area(1)，area(2)，...，area(m)，...，area(M))·(y₁，y₂，...，y_m，...，y_M)^T≥Area 公式(2)

y_m∈{0，1}，i＝1，2，...，m，...，M 公式(3)

其中y_m為變量，表示B中的第m臺機(jī)器人是否被分配到該覆蓋任務(wù)，如果是，y_m＝1，否則y_m＝0；area(m)表示機(jī)器人集合B中的第m臺機(jī)器人的最大覆蓋面積，該值等于第m臺機(jī)器人的電池續(xù)航時間、最大運(yùn)行速度及裝載的傳感器的感知范圍的乘積；

5.2.2求解基于機(jī)器人覆蓋面積的數(shù)學(xué)模型，其最優(yōu)解對應(yīng)分配給當(dāng)前覆蓋任務(wù)的最優(yōu)的K臺機(jī)器人，組成最優(yōu)機(jī)器人集合C＝{c₁,c₂,…,c_k,…,c_K}；

5.2.3計算C內(nèi)K臺機(jī)器人實(shí)際需要覆蓋的面積，得到面積集合CA，CA＝{ca₁,ca₂,…,ca_k,…,ca_K}，ca_k為第k臺機(jī)器人實(shí)際需要覆蓋的面積，ca_k按公式(4)計算如下：

其中CArea表示C中K臺機(jī)器人的最大覆蓋面積累加和，area(k)表示C中的第k臺機(jī)器人的最大覆蓋面積，area(k)等于第k臺機(jī)器人的電池續(xù)航時間、最大運(yùn)行速度及裝載的傳感器的感知范圍的乘積；

第六步，環(huán)境建模模塊從客戶端接收目標(biāo)區(qū)域的信息，對目標(biāo)區(qū)域進(jìn)行環(huán)境建模，從機(jī)器人選擇模塊接收C和CA，計算C中K臺機(jī)器人需要覆蓋的網(wǎng)格數(shù)量，統(tǒng)計與C中K臺機(jī)器人的位置距離最近的網(wǎng)格，方法是：

6.1采用STC網(wǎng)格建模法對目標(biāo)區(qū)域進(jìn)行網(wǎng)格建模，得到長為X，寬為Y的二維網(wǎng)格地圖，用二維矩陣D表示，D＝{d(x,y),1≤x≤X,1≤y≤Y,X和Y為正整數(shù)}，d(x,y)是橫坐標(biāo)為x，縱坐標(biāo)為y的網(wǎng)格的值，如果d(x,y)＝0，表示該網(wǎng)格為障礙物，如果d(x,y)＝1，表示該網(wǎng)格屬于待覆蓋區(qū)域；

6.2計算C內(nèi)K臺機(jī)器人需要覆蓋的網(wǎng)格數(shù)量，用網(wǎng)格數(shù)量集合G＝{g₁,g₂,…,g_k,…,g_K}表示，C中第k臺機(jī)器人需要覆蓋的網(wǎng)格數(shù)量g_k按公式(5)計算：

其中T表示機(jī)器人上搭載的傳感器的視野面積；將G中K個元素分別除以這K個元素的最大公約數(shù)，得到集合C中K臺機(jī)器人的網(wǎng)格數(shù)量比值，用網(wǎng)格數(shù)量比值集合H＝{h₁,h₂,…,h_k,…,h_K}表示，h_k是C中第k臺機(jī)器人的網(wǎng)格數(shù)量比值；

6.3統(tǒng)計二維矩陣D中，與C中K臺機(jī)器人的位置距離最近的K個網(wǎng)格，組成起始網(wǎng)格集合S＝{s(x₁,y₁),…,s(x_k,y_k),…,s(x_K,y_K)}，s(x_k,y_k)是C中第k臺機(jī)器人的起始網(wǎng)格；

第七步，區(qū)域劃分模塊從環(huán)境建模模塊接收最優(yōu)機(jī)器人集合C及其預(yù)期需要覆蓋的網(wǎng)格數(shù)量集合G、網(wǎng)格數(shù)量比值集合H、起始網(wǎng)格集合S以及二維網(wǎng)格矩陣D，從客戶端獲取策略選擇參數(shù)Stra，通過初始劃分和二次劃分兩個子模塊，將目標(biāo)區(qū)域劃分成K個待覆蓋子區(qū)域，將K個待覆蓋子區(qū)域發(fā)送給并行路徑規(guī)劃模塊，方法是：

7.1初始劃分模塊將二維矩陣D初步劃分為K個子區(qū)域，用初始子區(qū)域集合P＝{p₁,p₂,…,p_k,…,p_K}表示，p_k是C中第k臺機(jī)器人需要覆蓋的網(wǎng)格集合，具體方法如下：

7.1.1將集合C中K臺機(jī)器人的起始網(wǎng)格作為第一個分配網(wǎng)格分配到初始子區(qū)域集合P中；

7.1.2參考C中各機(jī)器人的預(yù)期網(wǎng)格數(shù)量G及網(wǎng)格數(shù)量比值H，交替將二維網(wǎng)格矩陣D中其他未分配網(wǎng)格分配給各子區(qū)域，方法是：

7.1.2.1令機(jī)器人序號k＝1；

7.1.2.2計算G中各元素的累加和若說明二維網(wǎng)格矩陣D中的待覆蓋網(wǎng)格均已被分配給C中的K臺機(jī)器人，轉(zhuǎn)7.2；否則，說明二維網(wǎng)格矩陣D中還有網(wǎng)格尚未被分配給C中的K臺機(jī)器人，轉(zhuǎn)7.1.2.3；

7.1.2.3統(tǒng)計和子區(qū)域p_k相鄰的未被分配網(wǎng)格，定義D中的未分配網(wǎng)格d(x1，y1)與子區(qū)域p_k相鄰，x1為橫坐標(biāo)，y1為縱坐標(biāo)，1≤x1≤X，1≤y1≤Y，當(dāng)且僅當(dāng)子區(qū)域p_k中存在網(wǎng)格d(x，y)滿足以下條件，

d(x1，y1)≠0 公式(7)

將與子區(qū)域p_k相鄰且未被分配的網(wǎng)格組成子區(qū)域p_k的候選網(wǎng)格集合E_k，E_k＝{e(x₁，y₁)，e(x₂，y₂)，...，e(x_q，y_q)，...e(x_Q，y_Q)}，e(x_q，y_q)≠0，e(x_q，y_q)∈D，Q為正整數(shù)，1≤q≤Q；

7.1.2.4根據(jù)從客戶端獲取的策略選擇參數(shù)Stra，從候選網(wǎng)格集合E_k中挑選出h_k個網(wǎng)格，分配給子區(qū)域p_k，方法如下：

7.1.2.4.1若Stra＝1，從候選網(wǎng)格集合E_k中，隨機(jī)挑選h_k個網(wǎng)格，組成子區(qū)域p_k的網(wǎng)格集合F_k，將F_k分配給子區(qū)域p_k，即p_k＝{p_k，F(xiàn)}，轉(zhuǎn)7.1.2.5；

7.1.2.4.2若Stra＝2，從候選網(wǎng)格集合E_k中，依次選取與起始網(wǎng)格s_k距離最近的h_k個網(wǎng)格，組成子區(qū)域p_k的網(wǎng)格集合F_k，將F_k分配給子區(qū)域p_k，即p_k＝{p_k，F(xiàn)}，轉(zhuǎn)7.1.2.5；

7.1.2.5令g_k＝g_k-h_k，k＝k+1；

7.1.2.6若k≤K，轉(zhuǎn)7.1.2.2；否則，轉(zhuǎn)7.1.2.1；

7.2二次劃分模塊從初始區(qū)域劃分模塊接收初始子區(qū)域集合P，通過二次劃分將二維網(wǎng)格矩陣D精確地劃分成K個子區(qū)域，這K個子區(qū)域組成精確子區(qū)域集合P’，并將P’輸出到并行路徑規(guī)劃模塊，方法是：

7.2.1令P’＝P，其中P’＝{p’₁，p’₂，...，p’_k，...，p’_K}，即使用P初始化P’；

7.2.2計算P’中K個子區(qū)域的實(shí)際分配網(wǎng)格數(shù)量與預(yù)期分配網(wǎng)格數(shù)量之間的偏差，得到網(wǎng)格數(shù)量偏差集合G’；

7.2.3若max(G’)＝0，說明P’中K個子區(qū)域的實(shí)際分配網(wǎng)格數(shù)量與預(yù)期分配網(wǎng)格數(shù)量相等，max(G’)為對集合G’中的元素取最大值，轉(zhuǎn)7.2.9；否則，統(tǒng)計G中數(shù)值等于±m(xù)ax(G’)的子區(qū)域，構(gòu)成最大偏差子區(qū)域集合PG＝{pg₁，pg₂，...，pg_i，...，pg_I}，I為正整數(shù)，1≤I≤K，1≤i≤I，pg_i為PG中的第i個子區(qū)域，轉(zhuǎn)7.2.4；

7.2.4建立搜索樹，從PG中隨機(jī)挑選出一個元素pg_i作為該搜索樹的根節(jié)點(diǎn)，然后逐級生成搜索樹的各層子節(jié)點(diǎn)，直到在某層子節(jié)點(diǎn)中找到可以和pg_i進(jìn)行二次再分配的子區(qū)域?yàn)橹梗环椒ㄈ缦拢?/p>

7.2.4.1令搜索樹節(jié)點(diǎn)層數(shù)j＝2，令搜索樹第j-1層節(jié)點(diǎn)集合T＝{pg_i}；

7.2.4.2生成搜索樹第j層節(jié)點(diǎn)，組成第j層節(jié)點(diǎn)集合TT，TT中包含子區(qū)域集合TR，TR＝{tr₁，...，tr_o，...，tr_O}，O為正整數(shù)且1≤O≤K，1≤o≤O，tr_o為TR中的第o個子區(qū)域；

7.2.4.3若TT中存在子區(qū)域tr_o，滿足如果有，說明有可以和pg_i進(jìn)行二次再分配的子區(qū)域，這些子區(qū)域組成二次分配子區(qū)域集合TTS，從TTS中隨機(jī)挑選一個元素，令為tr_o，轉(zhuǎn)7.2.5；否則轉(zhuǎn)7.2.4.4；

7.2.4.4令j＝j(luò)+1，T＝TT，轉(zhuǎn)7.2.4.2；

7.2.5從節(jié)點(diǎn)tr_o開始，沿著搜索樹逐級回溯，直到遇到根節(jié)點(diǎn)pg_i，回溯過程中遇到的節(jié)點(diǎn)，組成回溯路徑子區(qū)域集合W＝{w₁，w₂，...，w_z，...，w_Z}，Z為正整數(shù)且1≤Z≤K，1≤z≤Z，其中w₁＝tr_o，w_Z＝pg_i；

7.2.6若說明子區(qū)域pg_i的實(shí)際網(wǎng)格分配數(shù)量比預(yù)期分配數(shù)量要少，轉(zhuǎn)7.2.7；否則，說明子區(qū)域pg_i的實(shí)際網(wǎng)格分配數(shù)量比預(yù)期分配數(shù)量要多，轉(zhuǎn)7.2.8；

7.2.7從其他子區(qū)域中挑選出一個網(wǎng)格，二次分配給子區(qū)域pg_i；方法如下：

7.2.7.1令實(shí)際二次分配子區(qū)域集合W1＝{w₁，w₂，...，w_z，...，w_Z-1}，令W1中子區(qū)域序號z＝1；

7.2.7.2從子區(qū)域w_z中挑選出一個網(wǎng)格d(x，y)，轉(zhuǎn)移給子區(qū)域w_z+1中即令w_z+1＝{w_z+1，d(x，y)}，w_z＝w_z-{d(x，y)}；挑選的網(wǎng)格滿足以下條件：二次分配完以后，子區(qū)域{w_z+1，d(x，y)}依然是連通的，定義子區(qū)域是連通的，當(dāng)且僅當(dāng)該子區(qū)域內(nèi)的每個網(wǎng)格，都存在至少一個相鄰網(wǎng)格；

7.2.7.3令z＝z+1；

7.2.7.4若z＝Z-1，令轉(zhuǎn)7.2.2；否則，轉(zhuǎn)7.2.7.2；

7.2.8將子區(qū)域pg_i內(nèi)部的一個網(wǎng)格，二次分配給其他子區(qū)域，方法如下：

7.2.8.1令W1＝{w₂,…,w_z,…,w_Z}，令z＝1；

7.2.8.2從子區(qū)域w_z+1中挑選出一個網(wǎng)格，轉(zhuǎn)移給子區(qū)域w_z，即w_z＝{w_z,d(x,y)}，w_z+1＝w_z+1-{d(x,y)}；挑選的網(wǎng)格滿足以下條件：二次分配完以后，子區(qū)域依然是連通的；

7.2.8.3令z＝z+1；

7.2.8.4若z＝Z-1，則令轉(zhuǎn)7.2.2；否則，轉(zhuǎn)7.2.8.2；

7.2.9經(jīng)過二次劃分，得到精確的子區(qū)域集合P’，將P’發(fā)送給并行路徑規(guī)劃模塊；

第八步，并行路徑規(guī)劃模塊從區(qū)域劃分模塊接收精確子區(qū)域集合P’，在P’的K個子區(qū)域上并行執(zhí)行單機(jī)器人生成樹覆蓋路徑規(guī)劃算法，在K個子區(qū)域分別生成一條覆蓋路徑，共生成K條覆蓋路徑，將K條覆蓋路徑分別發(fā)送給與C對應(yīng)的K臺機(jī)器人終端；

第九步，與C對應(yīng)的K臺機(jī)器人終端并行接收對應(yīng)的覆蓋路徑，并行按照對應(yīng)的覆蓋路徑運(yùn)動，生成K條運(yùn)動軌跡，這K條運(yùn)動軌跡實(shí)現(xiàn)對目標(biāo)區(qū)域的完全覆蓋。