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

有效

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

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

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

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

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

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

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

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

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

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

(2)設定起點和終點動態(tài)添加的規(guī)則，起點動態(tài)添加規(guī)則為：先添加圖G中起點周圍的節(jié)點作為起點下一步可達點，再篩選出這些節(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分別表示實際地圖中的橫坐標與縱坐標，單位為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分別對應著上述start、goal、point2，P為待求取的路徑上的點，t取值范圍為[0,1]的變量；

步驟4、設計任務的分配，多移動機器人調(diào)度平臺負責向多移動機器人發(fā)送總?cè)蝿眨嘁苿訖C器人在接收到總?cè)蝿蘸箝_始協(xié)商并分配任務，任務分配方式采用多輪分配的方式，每個移動機器人每一輪只分配下一刻要執(zhí)行的任務，在移動機器人完成當前任務后，再開始下一輪任務分配，直至各移動機器人總?cè)蝿詹蹫榭詹磐Ｖ谷蝿辗峙洌苿訖C器人每輪任務協(xié)商、分配，具體包括以下子步驟：

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

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

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

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

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

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

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

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

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

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

步驟6、開發(fā)多移動機器人調(diào)度平臺軟件，通過Qt以及ROS開發(fā)多移動機器人調(diào)度平臺軟件，設計總?cè)蝿障⒏袷剑瑒?chuàng)建總?cè)蝿瞻l(fā)布功能以及多移動機器人監(jiān)控界面，移動機器人監(jiān)控界面中加載實驗環(huán)境地圖，以不同顏色的圓形圖標表示不同的移動機器人，測試和監(jiān)控多移動機器人調(diào)度及運行效果；設計總?cè)蝿蛰斎肟颍奢斎肽M收集到的總?cè)蝿眨ㄟ^點擊“Send”按鍵向多移動機器人發(fā)送總?cè)蝿眨辉O置系統(tǒng)停止運行按鈕“Stop”,以便出現(xiàn)緊急狀況時，能夠即時停止系統(tǒng)的運行。