[發(fā)明專利]一種基于多傳感器的多移動機(jī)器人調(diào)度系統(tǒng)設(shè)計方法

有效

• 申請?zhí)枺?/td>: 202010035287.7
• 申請日: 2020-01-14
• 公開（公告）號: CN111103887B
• 公開（公告）日: 2021-11-12
• 發(fā)明（設(shè)計）人: 王東;劉斌祺;朱理;連捷;于連波
• 申請（專利權(quán)）人: 大連理工大學(xué)
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 大連星海專利事務(wù)所有限公司 21208
• 代理人: 王樹本;徐雪蓮

1.一種基于多傳感器的多移動機(jī)器人調(diào)度系統(tǒng)設(shè)計方法，其特征在于包括以下步驟：

步驟1、搭建多個移動機(jī)器人平臺，以TurtleBot2輪式機(jī)器人作為移動底盤，在移動底盤周圍外側(cè)設(shè)置有紅位姿識別標(biāo)靶，紅位姿識別標(biāo)靶前側(cè)即機(jī)器人前側(cè)設(shè)置有兩道紅色條紋，紅位姿識別標(biāo)靶后側(cè)設(shè)置有單道紅色條紋，紅位姿識別標(biāo)靶前、后側(cè)連接處留出長度為1.5-1.6cm的空白區(qū)；在移動底盤上設(shè)置有兩層亞克力板并與移動底盤形成三層空間，將NVIDIA Jeston TX2開發(fā)板作為上位機(jī)，安裝在第二層中間位置，同時將Intel RealsenseZR300深度實感相機(jī)安裝第二層前側(cè)正中間；在第三層搭載RPLIDAR-A3激光雷達(dá)；

步驟2、構(gòu)建二維實驗場景地圖及實現(xiàn)多移動機(jī)器人無線通訊，包含以下子步驟：

(a)控制移動機(jī)器人在實驗場景范圍內(nèi)移動，利用RPLIDAR-A3激光雷達(dá)掃描環(huán)境信息，通過cartographer建圖算法構(gòu)建二維的實驗場景地圖；

(b)以一臺臺式電腦作為多移動機(jī)器人調(diào)度平臺，設(shè)置調(diào)度平臺與移動機(jī)器人之間的信息交互方式，使調(diào)度平臺與移動機(jī)器人之間的交互方式為調(diào)度平臺發(fā)送總?cè)蝿?wù)信息給移動機(jī)器人、移動機(jī)器人反饋位置信息給調(diào)度平臺；使移動機(jī)器人之間的信息交互方式為移動機(jī)器人之間任務(wù)信息相互交換；

(c)定義任務(wù)消息格式，基于TCP/IP協(xié)議在移動機(jī)器人及調(diào)度平臺上創(chuàng)建基于不同網(wǎng)絡(luò)端口的TCP服務(wù)器及客戶端，在機(jī)器人之間及機(jī)器人與調(diào)度平臺之間建立起TCP連接，通過socket實現(xiàn)任務(wù)信息、機(jī)器人位置信息、總?cè)蝿?wù)消息的收發(fā)；

(d)在移動機(jī)器人上創(chuàng)建兩個ROS節(jié)點，一個用于將其它移動機(jī)器人的任務(wù)消息通過ROS話題發(fā)布，另一個用來將不同移動機(jī)器人的任務(wù)消息同步訂閱下來，用作任務(wù)協(xié)商；

步驟3、全局路徑規(guī)劃的設(shè)計，基于實驗場景地圖創(chuàng)建一個簡化地圖，結(jié)合A*算法和二階Bezier曲線實現(xiàn)全局路徑規(guī)劃；全局路徑規(guī)劃具體執(zhí)行過程包括以下四個方面的內(nèi)容，一是將起點與終點動態(tài)地加入簡化地圖，二是在簡化地圖上利用A*算法計算得到一條粗略路徑，三是基于實際地圖，在粗略路徑的相鄰節(jié)點間，通過A*算法以及二階Bezier曲線得到各節(jié)點間的精確路徑，四是將各節(jié)點間的路徑按節(jié)點順序串聯(lián)起來作為最終路徑；全局路徑規(guī)劃具體包含以下三個設(shè)計要點：

(1)基于實驗場景地圖上創(chuàng)建一個簡單有向圖G(V,E)，即簡化地圖，圖中節(jié)點V對應(yīng)著實際地圖中特定的坐標(biāo)點，主要分布在道路的兩側(cè)及彎道處，節(jié)點之間的邊E為有向邊，用于限制移動機(jī)器人路徑規(guī)劃時的路徑方向；

(2)設(shè)定起點和終點動態(tài)添加的規(guī)則，起點動態(tài)添加規(guī)則為：先添加圖G中起點周圍的節(jié)點作為起點下一步可達(dá)點，再篩選出這些節(jié)點中滿足存在出邊向外的節(jié)點，即有出邊不是指向這些周圍節(jié)點的節(jié)點，作為最終與起點相連的節(jié)點；終點動態(tài)添加規(guī)則與前者唯一不同的地方在于它是篩選出周圍節(jié)點中滿足存在外來入邊的點作為最終與終點相連的節(jié)點，即有入邊來自于周圍節(jié)點之外的節(jié)點的節(jié)點；

(3)在大弧度轉(zhuǎn)彎處采用二階Bezier曲線構(gòu)造路徑，首先，判斷是否位于大弧度轉(zhuǎn)彎處，即判斷是否滿足式(1)條件，

式中，start、goal分別表示粗略路徑中相鄰節(jié)點中的起點和終點，x、y分別表示實際地圖中的橫坐標(biāo)與縱坐標(biāo)，單位為m，K_sg表示起點與終點間的斜率，表示起點與粗略路徑中起點的前一節(jié)點p_s間的斜率；其次，若滿足上述條件，則確定二階Bezier曲線中的定點point2，即直線與的交點為point2，為粗略路徑中過goal和goal下一個節(jié)點n_g的直線，若該交點位于障礙區(qū)，則將該點沿著與直線l_sg相互垂直的方向移動，直至其位于無障礙區(qū)；最后，再利用二階Bezier曲線構(gòu)造公式(2)求解構(gòu)造彎道局部路徑的點，

P＝(1-t)²S+2t(1-t)P₂+t²G (2)

式中，S、P₂、G分別對應(yīng)著上述start、goal、point2，P為待求取的路徑上的點，t取值范圍為[0,1]的變量；

步驟4、設(shè)計任務(wù)的分配，多移動機(jī)器人調(diào)度平臺負(fù)責(zé)向多移動機(jī)器人發(fā)送總?cè)蝿?wù)，多移動機(jī)器人在接收到總?cè)蝿?wù)后開始協(xié)商并分配任務(wù)，任務(wù)分配方式采用多輪分配的方式，每個移動機(jī)器人每一輪只分配下一刻要執(zhí)行的任務(wù)，在移動機(jī)器人完成當(dāng)前任務(wù)后，再開始下一輪任務(wù)分配，直至各移動機(jī)器人總?cè)蝿?wù)槽為空才停止任務(wù)分配，移動機(jī)器人每輪任務(wù)協(xié)商、分配，具體包括以下子步驟：

(a)在接受到總?cè)蝿?wù)信息后獲取自身位置信息，根據(jù)總?cè)蝿?wù)信息中各任務(wù)的起止點信息，從移動機(jī)器人角度考慮，選取代價最小的n個任務(wù)t，n為移動機(jī)器人數(shù)量；有3個移動機(jī)器人，故取n為3，選取3個代價最小的任務(wù)組成自身任務(wù)序列l(wèi)₁＝(t₁,t₂,t₃)^T，任務(wù)選取可根據(jù)移動機(jī)器人自身約束條件以及任務(wù)條件建立代價函數(shù)，通過式(3)進(jìn)行描述，

c(i)＝distance(cur,start_i)+distance(start_i,goal_i) (3)

式中c(i)表示執(zhí)行任務(wù)i所花費的代價值大小，cur表示移動機(jī)器人的當(dāng)前位置，start_i表示任務(wù)i起點，goal_i表示任務(wù)i終點，distance表示兩點間的曼哈頓距離；

(b)將自身任務(wù)序列打包成任務(wù)消息發(fā)送給其它移動機(jī)器人，同時接收來自于其它移動機(jī)器人的任務(wù)序列l(wèi)₂、l₃，若一段時間T內(nèi)未接收到移動機(jī)器人i發(fā)送過來的任務(wù)序列，則取上次協(xié)商完成后機(jī)器人i分配的任務(wù)t′，重新構(gòu)建一個任務(wù)序列l(wèi)_i＝(t′,t′,t′)，并將任務(wù)序列l(wèi)_i作為移動機(jī)器人i此刻的任務(wù)序列，然后，將步驟(a)中自身挑選的任務(wù)序列與接收到的其它移動機(jī)器人的任務(wù)序列匯總到一個任務(wù)序列集S＝(l₁,l₂,l₃)；

(c)根據(jù)任務(wù)爭取規(guī)則，若存在多個移動機(jī)器人爭取同一個任務(wù)t情況，則選取離任務(wù)t起點近的機(jī)器人作為任務(wù)t的接收者，從任務(wù)序列集S第一行開始，根據(jù)各移動機(jī)器人與任務(wù)起點之間的距離信息，循環(huán)比較任務(wù)序列集S中的每一行元素值即任務(wù)標(biāo)號，直到某行的所有元素均互異為止，此時每個移動機(jī)器人都能分配到不同的任務(wù)，倘若當(dāng)前移動機(jī)器人對應(yīng)的任務(wù)序列為l_j，第i行各元素的比較過程存在以下兩種情況：若任務(wù)l_ij異于所在行的第i行的其它元素，則將任務(wù)l_ij分配給當(dāng)前移動機(jī)器人j，并將l_j中當(dāng)?shù)趇個元素以后的元素均置為當(dāng)前元素值l_ij；若第i行存在與任務(wù)l_ij相同的元素，則按照任務(wù)爭取規(guī)則，將任務(wù)l_ij分配給距離任務(wù)l_ij起點最近的移動機(jī)器人k，即令l_ik等于l_ij，將第i行中其它與l_ik元素相同的元素置0值，同時將任務(wù)序列l(wèi)_k中第i行以后的元素置為l_ik值；

步驟5、多移動機(jī)器人預(yù)防碰撞策略的設(shè)計，一方面，通過RPLIDAR-A3激光雷達(dá)實時掃描移動機(jī)器人周圍環(huán)境信息，當(dāng)檢測到行進(jìn)路線上出現(xiàn)障礙物時，在障礙物的一定范圍內(nèi)重新規(guī)劃局部路徑，調(diào)整當(dāng)前全局路徑；另一方面，通過ZR300深度實感相機(jī)實時檢測移動機(jī)器人周圍的其它移動機(jī)器人，并對其位姿進(jìn)行識別，再根據(jù)減速策略對移動機(jī)器人速度進(jìn)行調(diào)節(jié)，以此保證移動機(jī)器人之間的相對安全性，包括紅色位姿識別標(biāo)靶檢測及減速策略的設(shè)計；

(1)紅色位姿識別標(biāo)靶檢測，通過ZR300深度實感相機(jī)采集移動機(jī)器人正前方的環(huán)境信息，在ZR300深度實感相機(jī)輸出的RGB圖像基礎(chǔ)上根據(jù)紅色位姿識別標(biāo)靶的顏色和形狀特征檢測紅色位姿識別標(biāo)靶，紅色位姿識別標(biāo)靶共有三種姿態(tài)，每種姿態(tài)都具有不同的矩形形態(tài)，且矩形之間存在一定的空間位置關(guān)系，姿態(tài)一為單個矩形形態(tài)，姿態(tài)二為三個矩形形態(tài)，姿態(tài)三為兩個矩形形態(tài)；若檢測到紅色識別標(biāo)靶，則先求出紅色識別標(biāo)靶整體的形心在圖像坐標(biāo)系下的位置，記作(u_c,v_c)；然后，將此刻ZR300深度實感相機(jī)采集到的RGB圖像與深度圖對齊，讀取深度圖中對應(yīng)的形心位置處的深度值，記作Z_c；最后，根據(jù)相機(jī)的內(nèi)參數(shù)矩陣K求出圖像坐標(biāo)系下的紅色位姿識別標(biāo)靶形心在相機(jī)坐標(biāo)系下的空間位置P＝(X_c,Y_c,Z_c)^T，通過式(4)進(jìn)行描述，

估計移動機(jī)器人在相機(jī)坐標(biāo)系下的姿態(tài)，由于所有移動機(jī)器人均在同一地圖平面上，所以也就是估計歐拉角rpy中的偏航角yaw；當(dāng)檢測到的移動機(jī)器人姿態(tài)為姿態(tài)一時，說明此時移動機(jī)器人正處于背對著相機(jī)的狀態(tài)，移動機(jī)器人坐標(biāo)系與相機(jī)坐標(biāo)系x軸方向一致，yaw設(shè)為0°；當(dāng)檢測到的移動機(jī)器人姿態(tài)為位姿三時，說明此時移動機(jī)器人與相機(jī)正處于面對面的狀態(tài)，此時yaw設(shè)為180°；當(dāng)檢測到的移動機(jī)器人姿態(tài)為姿態(tài)二時，說明此時移動機(jī)器人相對于相機(jī)正處于橫行的狀態(tài)，可根據(jù)移動機(jī)器人底盤半徑、行走方向、紅色位姿識別標(biāo)靶的轉(zhuǎn)向標(biāo)識點與形心之間的距離計算yaw，轉(zhuǎn)向標(biāo)志點即紅色位姿識別標(biāo)靶前側(cè)與后側(cè)銜接區(qū)域的中心位置，通過下式(5)進(jìn)行描述，

式(5)中dir代表識別到的目標(biāo)機(jī)器人的方向，當(dāng)dir等于0時，表示目標(biāo)機(jī)器人正處于姿態(tài)一或姿態(tài)三狀態(tài)；當(dāng)dir大于0時，表示目標(biāo)機(jī)器人正處于向右狀態(tài)；當(dāng)dir小于0時，表示機(jī)器人正處于向左的狀態(tài)；分別表示標(biāo)靶轉(zhuǎn)向標(biāo)識點和標(biāo)靶中心點在相機(jī)坐標(biāo)系下的y坐標(biāo)；

(2)減速策略的設(shè)計，當(dāng)與識別到的移動機(jī)器人之間的距離大于2m時，保持原有最大移動速度；當(dāng)與識別到的移動機(jī)器人之間的距離小于2m，但自身接下來仍然會處于直行狀態(tài)時或者接下來行走方向跟識別到的移動機(jī)器人當(dāng)前行走方向相反，則依舊保持原有最大移動速度；排除以上兩種情況后，當(dāng)與識別到的移動機(jī)器人之間的距離d小于2m且大于0.7m時，移動機(jī)器人的最大速度隨著距離d的減小而減小，保證距離d越小，最大移動速度減小的幅度就越大；當(dāng)與識別到的移動機(jī)器人之間的距離d小于0.7m時，最大移動速度強制設(shè)置為0，停止當(dāng)前移動機(jī)器人運行；

步驟6、開發(fā)多移動機(jī)器人調(diào)度平臺軟件，通過Qt以及ROS開發(fā)多移動機(jī)器人調(diào)度平臺軟件，設(shè)計總?cè)蝿?wù)消息格式，創(chuàng)建總?cè)蝿?wù)發(fā)布功能以及多移動機(jī)器人監(jiān)控界面，移動機(jī)器人監(jiān)控界面中加載實驗環(huán)境地圖，以不同顏色的圓形圖標(biāo)表示不同的移動機(jī)器人，測試和監(jiān)控多移動機(jī)器人調(diào)度及運行效果；設(shè)計總?cè)蝿?wù)輸入框，可輸入模擬收集到的總?cè)蝿?wù)，通過點擊“Send”按鍵向多移動機(jī)器人發(fā)送總?cè)蝿?wù)；設(shè)置系統(tǒng)停止運行按鈕“Stop”,以便出現(xiàn)緊急狀況時，能夠即時停止系統(tǒng)的運行。