基于已知環(huán)境下的移動機器人路徑規(guī)劃方法
【專利摘要】本發(fā)明涉及基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，包括以下步驟：1、記錄點坐標和轉(zhuǎn)向角；2、生成地圖拓撲圖；3、計算并填充有向圖；4、設(shè)置點的數(shù)組；5、遍歷各點；6、用Floyd算法計算最短路徑矩陣。本發(fā)明提出一種簡單可靠的已知環(huán)境下的機器人路徑規(guī)劃方法，本方法計算量小，規(guī)劃算法快速可靠，并可以實現(xiàn)多機器人單一軌道的路徑規(guī)劃，尤其適用在多機器人、單行道條件下的機器人協(xié)同工作。
【專利說明】
基于已知環(huán)境下的移動機器人路徑規(guī)劃方法
技術(shù)領(lǐng)域
[0001]本發(fā)明涉及機器人控制方法技術(shù)領(lǐng)域，具體的說是基于已知環(huán)境下的移動機器人路徑規(guī)劃方法。
【背景技術(shù)】
[0002]目前，依靠StarGazer等傳感器來實現(xiàn)機器人自主定位與導航，已經(jīng)成為目前各大主流服務(wù)機器人廠商的選擇，其原理是StarGazer傳感器通過不斷的讀取頂標并計算出機器人所在位置，來實現(xiàn)的定位，通過控制中心，判斷并指定機器人下一步需要到達的位置，最終來實現(xiàn)機器人導航。其中，通過對已知環(huán)境下的機器人進行路徑規(guī)劃與導航，一直是一個難以解決的問題。即機器人到達某一個指定點選擇一條可靠的最短路徑，而且實際情況下，此路徑往往都只是單行道，即一條路徑下只允許一個機器人的行走，當有多個機器人的時候，這種情況尤為難以解決。
[0003]目前，各大廠商利用的主要方法為基于Dijkstra的擴展，主要是求取兩點之間的最短路徑，并結(jié)合柵格法，可以實現(xiàn)機器人沿著最短線路行走。但目前主要缺點是機器人算法復雜，計算量大，無法動態(tài)調(diào)整已經(jīng)規(guī)劃好的路線，尤其無法解決多機器人的路徑規(guī)劃和動態(tài)調(diào)整。

【發(fā)明內(nèi)容】

[0004]為解決上述問題，本發(fā)明提供一種基于已知環(huán)境下的移動機器人路徑規(guī)劃方法。
[0005]—種基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，包括以下步驟:
[0006]第一步，采集坐標:需要獲取整個地圖的全局坐標，建立好全局坐標系后，需要讓機器人在整個空間走動一遍，并記錄機器人需要經(jīng)過的點，記錄點的相應(yīng)坐標，以及移動到下一點需要的轉(zhuǎn)向角，如果無法采集轉(zhuǎn)向角，貝lJ采用計算角，計算角需要加上一定的偏移量。
[0007]第二步，生成地圖拓撲圖:利用對應(yīng)關(guān)系，通過轉(zhuǎn)換實際坐標系到屏幕坐標系，將相應(yīng)的點生成對應(yīng)的地圖拓撲圖，以判斷相關(guān)點的對應(yīng)關(guān)系是否正確，以及判斷拓撲結(jié)構(gòu)是否合理。
[0008]第三步，計算并填充有向圖:將步驟二中的對應(yīng)關(guān)系整理成數(shù)據(jù)結(jié)構(gòu)中的有向圖；有向圖是數(shù)據(jù)結(jié)構(gòu)中的一種，具體包含一個二維數(shù)組，數(shù)組中的每個元素是一個包含兩點之間的權(quán)值和角度的結(jié)構(gòu)體，權(quán)值為兩點間計算的長度;有向圖結(jié)構(gòu)中包含兩個整形變量，一個變量用于存儲有向圖中點的個數(shù)，另一個用于存儲有向圖中邊的個數(shù);有向圖結(jié)構(gòu)中包含一個一維數(shù)組，用以校正對應(yīng)的點信息，數(shù)組的容量為有向圖中點的個數(shù)。
[0009]第四步，設(shè)置點的數(shù)組:設(shè)置一個點前驅(qū)的二維數(shù)組，一個最短距離的二維數(shù)組，一個點的終點角度數(shù)組，數(shù)組大小即為點的大小。點前驅(qū)的二維數(shù)組用來記錄到這個點的最短路徑的前一個應(yīng)該到達的點，最短距離的二維數(shù)組是存儲兩點之間的經(jīng)過最短路徑所經(jīng)過的距離，點的終點角度數(shù)組需要存儲點作為終點或者停止點的角度。
[0010]第五步，遍歷各點:初始化各變量，遍歷之前記錄的點的拓撲圖，如果兩點之間存在連接關(guān)系，將此兩點之間的距離信息設(shè)置為計算距離值，角度信息填寫為對應(yīng)角度，即轉(zhuǎn)向角度。若兩點之間沒有連接關(guān)系，設(shè)置為整形最大值，其中，由于是雙向圖，所以圖的矩陣非對稱矩陣，矩陣下標相等點為值O。
[0011]第六步，用Floyd算法計算最短路徑矩陣:利用Floyd算法計算出兩點之間的最短路徑，其原理是遍歷任意兩個點，從圖的帶權(quán)鄰接矩陣A= [a(i，j)]開始，遞歸地進行η次更新，即由矩陣D(O)=A,按一個公式，構(gòu)造出矩陣D(I);又用同樣地公式由D(I)構(gòu)造出D
(2)；……；最后又用同樣的公式由D(n-l)構(gòu)造出矩陣D(n)。矩陣D(n)的i行j列元素便是i號頂點到j(luò)號頂點的最短路徑長度，稱D(n)為圖的距離矩陣，同時還可引入一個后繼節(jié)點矩陣path來記錄兩點間的最短路徑，前驅(qū)二維數(shù)組存儲前驅(qū)點坐標，距離二維數(shù)據(jù)存儲最短距離。
[0012]當有機器人請求給定終點進行路徑查詢時，首先遍歷所有點，找出離起始點最近的采集點和終點最近的采集點，由前驅(qū)數(shù)組來找出從起始采集點O到終點N前的一個點M，再由前驅(qū)數(shù)組找出從起點到M點前驅(qū)點L，……，一直到查詢到前驅(qū)點為起始采集點O時停止。輸出所有經(jīng)過的路徑，發(fā)送到機器人讓機器人執(zhí)行。
[0013]當機器人開始啟動執(zhí)行時，首先將所有經(jīng)過路段“加鎖”，即將雙向圖中反方向路徑權(quán)值加上一個最大整形(如65535)，每當運行到一個點，將運行過的點進行“解鎖”，即將兩點之間的權(quán)值減去最大整形，這樣保證其他機器人每次檢索出來是一個相對的最端路徑，而且這條路徑不會有其他機器人沿著這條路徑反方向行走，當其他機器人查找路徑時，會讓機器人選擇繞行，而不是按原路返回，這樣在單行道場合完成最大化使用效率，滿足多機器人的單行道路徑規(guī)劃。
[0014]本發(fā)明的有益效果是:
[0015]本發(fā)明提出一種簡單可靠的已知環(huán)境下的機器人路徑規(guī)劃方法，本方法計算量小，規(guī)劃算法快速可靠，并可以實現(xiàn)多機器人單一軌道的路徑規(guī)劃，尤其適用在多機器人、單行道條件下的機器人協(xié)同工作。
【附圖說明】
[0016]下面結(jié)合附圖和實施例對本發(fā)明進一步說明。
[0017]圖1為本發(fā)明的控制流程圖；
[0018]圖2為本發(fā)明的路徑展示圖。
【具體實施方式】
[0019]為了使本發(fā)明實現(xiàn)的技術(shù)手段、創(chuàng)作特征、達成目的與功效易于明白了解，下面對本發(fā)明進一步闡述。
[0020]如圖1、圖2所示，基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，包括以下步驟:首先，需要利用StarGazer傳感器建立整個活動空間的全局坐標，具體建立過程詳情參考StarGazer傳感器手冊。本發(fā)明主要是利用此傳感器獲得整個地圖的全局坐標，但不僅僅局限于StarGazer傳感器。
[0021]建立好全局坐標系后，讓機器人在整個空間走動一遍，并記錄機器人需要經(jīng)過的點，這些點主要是機器人需要轉(zhuǎn)換角度或者停止的點。記錄點的相應(yīng)坐標，以及移動到下一點需要轉(zhuǎn)動的角度。記錄方式為1_{3，103;5，254;...}，表示點I與點3，點5等連接，從點I往點3移動時轉(zhuǎn)向角度為103度，從點3往點5移動時的轉(zhuǎn)向角度為254度等，點的命名從O開始，只需記錄點小5IJ點大的點，例如只需記錄I點到3點和5點的連接關(guān)系和角度，而3點和5點至IJI點的相關(guān)信息并不需要記錄。本發(fā)明可以利用安裝在機器人上的傳感器記錄并輸出相應(yīng)連接角度，也可以通過計算得到相應(yīng)的連接角度，但需要根據(jù)實際場景添加一定的角度偏移量。
[0022]第二步，利用對應(yīng)關(guān)系，通過轉(zhuǎn)換實際坐標系到屏幕坐標系，將相應(yīng)的點生成對應(yīng)的地圖拓撲圖，可以很直觀的判斷相關(guān)點的對應(yīng)關(guān)系是否正確，以及判斷拓撲結(jié)構(gòu)是否合理。
[0023]第三步，計算并填充有向圖:將步驟二中的對應(yīng)關(guān)系整理成數(shù)據(jù)結(jié)構(gòu)中的有向圖；有向圖是數(shù)據(jù)結(jié)構(gòu)中的一種，具體包含一個二維數(shù)組，數(shù)組中的每個元素是一個包含兩點之間的權(quán)值和角度的結(jié)構(gòu)體，權(quán)值為兩點間計算的長度;有向圖結(jié)構(gòu)中包含兩個整形變量，一個變量用于存儲有向圖中點的個數(shù)，另一個用于存儲有向圖中邊的個數(shù);有向圖結(jié)構(gòu)中包含一個一維數(shù)組，用以校正對應(yīng)的點信息，數(shù)組的容量為有向圖中點的個數(shù)。
[0024]第四步，準備一個點前驅(qū)的二維數(shù)組、一個最短距離的二維數(shù)組、一個點的終點角度數(shù)組，數(shù)組大小即為點的大小。點前驅(qū)的二維數(shù)組用來記錄到這個點的最短路徑的前一個應(yīng)該到達的點，最短距離的二維數(shù)組是存儲兩點之間的經(jīng)過最短路徑所經(jīng)過的距離，點的終點角度數(shù)組需要存儲點作為終點或者停止點的角度。
[0025]第五步，初始化各變量，遍歷之前記錄的點的拓撲圖，如果兩點之間存在連接關(guān)系，將此兩點之間的距離信息設(shè)置為計算距離值，角度信息填寫為對應(yīng)角度，即轉(zhuǎn)向角度。若兩點之間沒有連接關(guān)系，設(shè)置為整形最大值，其中，由于是雙向圖，所以圖的矩陣非對稱矩陣，矩陣下標相等點為值O。
[0026]第六步，利用Floyd算法計算出兩點之間的最短路徑，其原理是遍歷任意兩個點，從圖的帶權(quán)鄰接矩陣A = [a(i，j)]開始，遞歸地進行η次更新，S卩由矩陣D(O) = A，按一個公式，構(gòu)造出矩陣D(I);又用同樣地公式由D(I)構(gòu)造出D(2);……；最后又用同樣的公式由D(η-1)構(gòu)造出矩陣D(n)。矩陣D(n)的i行j列元素便是i號頂點到j(luò)號頂點的最短路徑長度，稱D(n)為圖的距離矩陣，同時還可引入一個后繼節(jié)點矩陣path來記錄兩點間的最短路徑，前驅(qū)二維數(shù)組存儲前驅(qū)點坐標，距離二維數(shù)據(jù)存儲最短距離。
[0027]當有機器人請求給定終點進行路徑查詢時，首先遍歷所有點，找出離起始點最近的采集點和終點最近的采集點，由前驅(qū)數(shù)組來找出從起始采集點O到終點N前的一個點M，再由前驅(qū)數(shù)組找出從起點到M點前驅(qū)點L，……，一直到查詢到前驅(qū)點為起始采集點O時停止。輸出所有經(jīng)過的路徑，發(fā)送到機器人讓機器人執(zhí)行。
[0028]當機器人開始啟動執(zhí)行時，首先將所有經(jīng)過路段“加鎖”，即將雙向圖中反方向路徑權(quán)值加上一個最大整形(如65535)，注意是加上最大整形，而不是無窮大，每當運行到一個點，將運行過的點進行“解鎖”，即將兩點之間的權(quán)值減去最大整形，這樣保證其他機器人每次檢索出來是一個相對的最端路徑。例如，路徑檢索出1-4-6-9，則先將9-6，6-4，4_1路徑的權(quán)值加上最大整形，為了防止經(jīng)過路段有機器人檢索到返回路徑。當機器人到達4點，則4-1路徑解鎖，到達終點9時，取終點角度數(shù)組中的角度，讓機器人轉(zhuǎn)動到指定角度。這種方法，一方面可以讓機器人會檢索到一條最短的路徑，而且這條路徑不會有其他機器人沿著這條路徑反方向行走，當其他機器人查找路徑時，會讓機器人選擇繞行，而不是按原路返回，這樣在單行道場合完成最大化使用效率。
[0029]以上顯示和描述了本發(fā)明的基本原理、主要特征和本發(fā)明的優(yōu)點。本行業(yè)的技術(shù)人員應(yīng)該了解，本發(fā)明不受上述實施例的限制，上述實施例和說明書中描述的只是本發(fā)明的原理，在不脫離本發(fā)明精神和范圍的前提下，本發(fā)明還會有各種變化和改進，這些變化和改進都落入要求保護的本發(fā)明內(nèi)。本發(fā)明要求保護范圍由所附的權(quán)利要求書及其等效物界定。
【主權(quán)項】
1.基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，其特征在于:包括以下步驟: 第一步，記錄點坐標和轉(zhuǎn)向角； 第二步，生成地圖拓撲圖:利用對應(yīng)關(guān)系，通過轉(zhuǎn)換實際坐標系到屏幕坐標系，將相應(yīng)的點生成對應(yīng)的地圖拓撲圖； 第三步，計算并填充有向圖:將步驟二中的對應(yīng)關(guān)系整理成數(shù)據(jù)結(jié)構(gòu)中的有向圖； 第四步，設(shè)置點的數(shù)組:設(shè)置一個點前驅(qū)的二維數(shù)組、一個最短距離的二維數(shù)組、一個點的終點角度數(shù)組，數(shù)組大小即為點的大小； 第五步，遍歷各點:初始化各變量，遍歷之前記錄的點的拓撲圖； 第六步，用Floyd算法計算最短路徑矩陣。2.根據(jù)權(quán)利要求1所述的基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，其特征在于:第一步中，所述轉(zhuǎn)向角為采集所得轉(zhuǎn)向角或計算角，計算角需要加上一定的偏移量。3.根據(jù)權(quán)利要求1或2所述的基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，其特征在于:第三步中，安裝在機器人上的傳感器為能夠輸出角度信息的傳感器，所述角度為記錄角度。4.根據(jù)權(quán)利要求1或2所述的基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，其特征在于:第三步中，安裝在機器人上的傳感器不能輸出角度信息，所述角度為計算角度，計算角度需根據(jù)實際需求添加一定的偏移量。5.根據(jù)權(quán)利要求1所述的基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，其特征在于:第四步中，點前驅(qū)的二維數(shù)組用來記錄到這個點的最短路徑的前一個應(yīng)該到達的點，最短距離的二維數(shù)組是存儲兩點之間的經(jīng)過最短路徑所經(jīng)過的距離，點的終點角度數(shù)組需要存儲點作為終點或者停止點的角度。6.根據(jù)權(quán)利要求1所述的基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，其特征在于:第五步中，由于拓撲圖是雙向圖，所以圖的矩陣非對稱矩陣，矩陣下標相等點為值O。7.根據(jù)權(quán)利要求1至6中任一項所述的基于已知環(huán)境下的移動機器人路徑規(guī)劃方法，其特征在于:當機器人開始啟動執(zhí)行時，先將所有經(jīng)過路段“加鎖”，每當運行到一個點，將運行過的點進行“解鎖”，當其他機器人查找路徑時，會讓機器人選擇繞行。
【文檔編號】G05D1/02GK106094834SQ201610569810
【公開日】2016年11月9日
【申請日】2016年7月19日
【發(fā)明人】郭龍, 陳雙, 范冬林, 胡超
【申請人】蕪湖哈特機器人產(chǎn)業(yè)技術(shù)研究院有限公司