一種基於雷射數據的移動機器人導航控制方法與流程
2023-04-30 14:10:16 2
本發明屬於智慧機器人導航領域，特別地涉及一種基於雷射數據的移動機器人導航控制方法。
背景技術：
在機器人研究領域中，機器人導航技術快速發展。隨著時代的進步以及科技水平的不斷提高，在環境中對機器人進行準確的定位也變得越來越重要，導航包含了多種不同的技術和應用：數據關聯、映射算法、定位算法和同步定位與地圖構建等。每一個步驟和細節可以通過不同的方法或傳感器獲取。
移動機器人導航方法主要可以分為兩種：一種是基於地圖的導航方法，另一種是無地圖導航方法。基於地圖的導航方法主要是通過自動或者半自動方法構建地圖，通常採用雷射測距儀、超聲波傳感器、或視覺系統來構建地圖。地圖主要有四種：柵格地圖、拓撲地圖、特徵地圖以及混合地圖。柵格地圖通常是由雷射測距儀產生的，有些方法使用超聲波傳感器也可以得到類似的柵格地圖。拓撲地圖的構建也是使用相同的傳感器但是地圖的思想不一樣。機器人在特定的位置記錄路標，使用路標之間的關聯來對機器人進行定位。大多數的特徵地圖的構建是通過照相機獲取然後用計算機視覺算法處理得到。然後，機器人通過對觀測值和期望值之間的比較對自身進行定位。如果映射方法能夠綜合以上兩種或三種方法或傳感器，稱之為混合地圖。
目前正在運行的移動機器人主要是採用磁軌跡規劃巡檢路線，以及無線射頻識別(RFID)等技術實現導航。這種方法需預先設定巡檢機器人的巡檢路線，然後在運行路線上鋪設磁軌道，並且在機器人停靠的位置預先設置RFID標識，存在地面施工複雜工作量大、機器人運行路線不靈活等問題。而採用二維雷射導航，不僅定位準確度高、成本低、不受光線影響，而且具備抗電磁幹擾，對配置系統要求不高等優點。
技術實現要素：
本發明的目的在於解決現有技術中存在的問題，並提供一種基於雷射數據的移動機器人導航控制方法。本發明所採用的具體技術方案如下:
基於雷射數據的移動機器人導航控制方法，包括如下步驟：
步驟1：在全局地圖中添加、設置路徑和停靠點；
步驟2：獲取移動機器人的實時定位信息；
步驟3：規劃移動機器人導航所需最短路徑；
步驟4：移動機器人沿所規劃路徑導航行駛，且在行駛過程中，實時計算機器人中心點到目標路徑的距離、與目標路徑的方向差以及中心點到目標節點的距離，並實時調整機器人的兩側輪速，使其逐漸靠近目標節點；
步驟5：當到達目標節點附近時，進行減速，使移動機器人停靠在目標節點附近；依次對後續的目標節點重複上述步驟，使移動機器人最終停靠在目標停靠點上。
作為優選，所述步驟1的具體過程為：
步驟1.1：進行地圖路徑設置，在點雲地圖中添加節點，同時連接節點繪製路徑；
步驟1.2：選擇一條已繪製的路徑，並在此路徑上添加點作為移動機器人導航時的一個停靠點；
步驟1.3：將各路徑的起始節點坐標、終止節點坐標、路徑長度、路徑方向信息存儲至資料庫中，同時也將停靠點的編號信息、坐標信息存儲至資料庫中。
作為優選，所述步驟2的具體過程為：
步驟2.1：移動機器人自帶的雷射傳感器以一定的頻率和角度範圍平面掃描外部環境，獲取雷射傳感器所在位置與周邊環境間的距離數據，構成此次掃描的點雲數據；
步驟2.2：移動機器人初始定位時，預設一個初始位置值作為迭代匹配算法的估計配準量，非初始定位時將前一幀點雲數據匹配所得的定位值作為後一幀雷射數據用於匹配的估計配準量；
步驟2.3：通過迭代匹配算法將掃描所得點雲數據與現有全局地圖的整體點雲數據進行匹配，獲得移動機器人上的雷射傳感器在全局地圖坐標系下的定位信息，包括X、Y軸坐標值(xL，yL)和方向角θL；
步驟2.4：將獲得的雷射傳感器的定位信息轉化為移動機器人中心點PR的坐標值(xR，yR)和方向角θR，並以中心點的定位值作為整個移動機器人的定位信息；
其中，xR＝xL-L·cosθL，yR＝yL-L·sinθL，θR＝θL,其中L為雷射傳感器到移動機器人中心點的平面距離。
迭代匹配算法可選擇現有技術中的方法，例如ICP算法，就是一種典型的迭代匹配算法。該算法是一種通過在迭代過程中不斷降低配準誤差來獲取採樣點相對位置變換的掃描匹配方法。主要分為兩大部分，對應點的搜索和變換參數的求解。整個算法就是在對對應點對的搜素——變換參數的計算——對應點進行剛性變換——檢驗目標函數值的迭代過程中不斷降低匹配誤差以獲得兩組數據間的相對位姿變化的最優變換參數(R,T)，其中R為旋轉矩陣，T為平移矩陣。
作為優選，所述步驟3的具體過程為：
步驟3.1：移動機器人獲取資料庫中該目標停靠點的信息；
步驟3.2：獲取移動機器人的初始定位信息(xR0，yR0，θR0)；
步驟3.3：通過最短路徑算法計算移動機器人的初始位置到目標停靠點位置Ps(xs，ys)運行距離最短的路徑組合；
步驟3.4：存儲步驟3.3中獲得的最短距離的路徑信息，包括所需途經的節點順序(P1，P2…Pn-1，Ps)和路徑順序(L1、L2…Ln-1，Ln)。
最短路徑算法可採用現有技術中的算法，例如Dijkstra算法，是典型的最短路徑算法，用於計算一個節點到其他所有節點的最短路徑。將地圖中規劃的所有路徑、節點、停靠點的組合視為帶權有向圖，每條路徑均為雙向，且各路徑長度為該路徑的權值。將機器人初始位置作為源點，通過Dijkstra算法計算源點到圖中其他各點的最短路徑，而後選出源點到目標停靠點的最短路徑。
作為優選，所述步驟4的具體過程為：
步驟4.1：移動機器人在導航初始時刻，將步驟3中得到的最短路徑中的第一個節點P1和第一條路徑L1分別作為目標節點和目標路徑；
步驟4.2：計算移動機器人當前方向角與目標路徑方向的角度差Δθ0，移動機器人以角度Δθ0進行原地旋轉，調整行進方向與目標路徑方向保持一致；
步驟4.3：移動機器人向前行進，在行進過程中實時計算機器人中心點PR(xR，yR)到目標路徑的垂直距離Δs、機器人行進方向與目標路徑的方向差Δθ以及機器人中心點到目標節點的直線距離d；例如，中心點到目標路徑的距離計算公式如下：其中，公式中目標路徑所在的直線路徑方程為Ax+By+C＝0；中心點到目標節點的距離公式如下：
步驟4.4：計算移動機器人輪速控制量ΔV＝ks·Δs+kθ·Δθ，其中ks,kθ分別為預設的距離Δs和方向差Δθ的權值；從而根據前一時刻的左輪輪速VL-before和右輪輪速VR-before，實時更新並控制移動機器人當前時刻的左輪輪速VL-after和右輪輪速VR-after，其中VL-after＝VL-before+ΔV，VR-after＝VR-before-ΔV。
作為優選，所述步驟5的具體過程為：
步驟5.1：在移動機器人行進過程中，判斷機器人中心與目標節點的距離d是否小於一定閾值d0，若d小於d0，則視為移動機器人已經到達目標節點附近；
步驟5.2：然後控制移動機器人進行減速，對左右輪速度增加動態控制量控制左輪輪速為V』L-after和右輪輪速為V』R-after，其中VL-after』＝kd·VL-after，VR-after』＝kd·VR-after；當移動機器人不斷靠近目標節點，即d越小時kd值也越小直至0，使移動機器人在目標節點處停下；
步驟5.3：將步驟3中得到的最短路徑中的下一個節點P2和下一條路徑L2分別作為目標節點和目標路徑，重複步驟4.2至步驟5.2；依次類推，直至運動至路徑Ln，並停靠在目標停靠點Ps。
本發明數據採集操作簡單，且無需預先了解環境結構空間，也無需額外鋪設地標，成本低廉、地圖創建速度快精度高，可實現移動機器人在室內室外環境下的準確導航控制。其他技術效果將在具體實施方式中進一步說明。
附圖說明
圖1全局地圖路徑規劃；
圖2移動機器人導航時各控制量示意圖。
具體實施方式
下面結合附圖和具體實施方式對本發明做進一步闡述，以便更好的理解本發明。本發明的技術特徵在沒有衝突的情況下，均可進行相互組合。
本實施方式中，移動機器人並沒有構造和型號上的限制，但其用於驅動的左輪和右輪需要能夠分別調整輪速。其正前端搭載用於掃描周邊環境的雷射傳感器，雷射傳感器中心與移動機器人中心點的連線與機器人的行進方向一致。
基於雷射數據的移動機器人導航控制方法，包括如下步驟和子步驟：
步驟1：全局地圖中添加和設置路徑，具體子步驟：
步驟1.1：首先構建一個用於控制機器人和輸入指令、信息的客戶端，通過客戶端進行地圖路徑設置，在點雲地圖中添加節點，同時連接節點繪製路徑。本實施例中地圖路徑節點如圖1所示。
步驟1.2：通過客戶端添加停靠點，選擇一條已繪製的路徑，並在此路徑上添加點作為移動機器人導航時的一個停靠點，該停靠點將作為最後一個目標節點。
步驟1.3：將各路徑的起始節點坐標、終止節點坐標、路徑長度、路徑方向信息存儲至資料庫中，同時也將停靠點的編號信息、坐標信息存儲至資料庫中，供機器人的控制系統調用。
步驟2：獲取移動機器人的實時定位信息，具體子步驟：
步驟2.1：移動機器人自帶的雷射傳感器以一定的頻率和角度範圍平面掃描外部環境，獲取雷射傳感器所在位置與周邊環境間的距離數據，構成此次掃描的點雲數據；
步驟2.2：移動機器人第一次開始定位時，為了提高定位的速度和準確性，可事先由用戶估計一個初始位置值，並輸入客戶端，然後將初始位置值作為迭代匹配算法的估計配準量。當非初始定位時，可以將前一幀點雲數據匹配所得的定位值作為後一幀雷射數據用於匹配的估計配準量；
步驟2.3：通過迭代匹配算法將掃描所得點雲數據與現有全局地圖的整體點雲數據進行匹配，獲得雷射傳感器在全局地圖坐標系下的定位信息，包括X、Y軸坐標值(xL，yL)和方向角θL(機器人行進方向與X軸的夾角)。在機器人後續的行進過程中，也通過該方法不斷地確定自身的定位信息，以計算各種參數。
本實施例中採用ICP算法作為迭代匹配算法。該算法的具體實現過程如下：
給定雷射數據點集P＝{Pi,i＝0,1,2,…,m}及全局地圖數據點集Q＝{Qi,i＝0,1,2,…,n}，設置最小誤差度量值ε和最大迭代次數D。
(1)估計配準：在數據點集P與Q匹配之前，將前一次迭代匹配完成後獲得的匹配結果作為P與Q的估計配準值。沒有估計配準，匹配算法會陷入局部極小值。
(2)尋找對應點：根據歐式距離最小原則尋找，歐式距離定義如下：
(3)對於集合Q中的各點，在集合P中找出距該點最近的對應點，且設集合P中由這些對應點組成的新點集為P『＝{Pi『,i＝0,1,2,…,n}。作為求解變換的匹配點。
(4)尋找變換(R,T)：使得兩組數據點間的誤差度量值最小，計算公式如下：
其中ω為旋轉弧度，旋轉矩陣平移向量Qi為參考點集，Pi』為對應的帶匹配的點集。通過最小化Ek，可以得到Tx、Ty和ω的解。
(5)應用變換，更新機器人位姿。用配準變換矩陣R、T進行坐標變換，得到新的點集Q1，即Q1＝RQ+T，計算Ek+1。
(6)迭代：當|Ek-Ek+1|>ε，重複步驟(3)-(5)。否則，跳出迭代，計算結束，匹配成功。
步驟2.4：由於雷射傳感器測量得到的定位信息實際上是其自身的定位，但其與移動機器人的中心點存在一定的位置差異，因此需要進行校正：首先測量雷射傳感器到移動機器人中心點的平面距離L，將步驟2.3中獲得的雷射傳感器的定位信息轉化為移動機器人中心點PR的坐標值(xR，yR)和方向角(θR)，並以中心點的定位值作為整個移動機器人的定位信息。
其中，xR＝xL-L·cosθL，yR＝yL-L·sinθL，θR＝θL。
步驟3：規劃移動機器人導航所需最短路徑。具體子步驟：
步驟3.1：通過客戶端確定目標停靠點，移動機器人獲取資料庫中該目標停靠點的信息；
步驟3.2：通過步驟2獲取移動機器人的初始定位信息(xR0，yR0，θR0)；
步驟3.3：通過最短路徑算法計算移動機器人初始位置到目標停靠點位置Ps(xs，ys)運行距離最短的路徑；
Dijkstra算法是典型的最短路徑算法，該算法的具體實現過程如下：
(1)算法思想：設G＝(V,E)是一個帶權有向圖，把圖中頂點集合V分成兩組，第一組為已求出最短路徑的頂點集合(用S表示，初始時S中只有一個源點，以後每求得一條最短路徑,就將加入到集合S中，直到全部頂點都加入到S中，算法就結束了)，第二組為其餘未確定最短路徑的頂點集合(用U表示)，按最短路徑長度的遞增次序依次把第二組的頂點加入S中。在加入的過程中，總保持從源點v到S中各頂點的最短路徑長度不大於從源點v到U中任何頂點的最短路徑長度。此外，每個頂點對應一個距離，S中的頂點的距離就是從v到此頂點的最短路徑長度，U中的頂點的距離，是從v到此頂點只包括S中的頂點為中間頂點的當前最短路徑長度。
(2)算法步驟：
a.初始時，S只包含源點，即S＝{v}，v的距離為0。U包含除v外的其他頂點，即:U＝{其餘頂點}，若v與U中頂點u有邊，則正常有權值，若u不是v的出邊鄰接點，則權值為∞。
b.從U中選取一個距離v最小的頂點m，把m，加入S中(該選定的距離就是v到m的最短路徑長度)。
c.以m為新考慮的中間點，修改U中各頂點的距離；若從源點v到頂點u的距離(經過頂點m)比原來距離(不經過頂點m)短，則修改頂點u的距離值，修改後的距離值的頂點m的距離加上邊上的權。
d.重複步驟b和c直到所有頂點都包含在S中。
步驟3.4：存儲步驟3.3中獲得的最短距離的路徑信息，包括所需途經的節點順序(P1，P2…Pn-1，Ps)和路徑順序(L1、L2…Ln-1，Ln)，Ps表示第s個節點，Ln表示第n個條路徑。
步驟4：移動機器人沿所規劃路徑導航行駛。如圖2，具體子步驟：
步驟4.1：移動機器人在導航初始時刻，將步驟3中得到的最短路徑中的第一個節點P1和第一條路徑L1分別作為目標節點和目標路徑，機器人第一步的目的是移動到該目標節點；
步驟4.2：計算移動機器人當前方向角與目標路徑方向的角度差Δθ0，移動機器人以角度Δθ0進行原地旋轉，調整自身方向與目標路徑方向保持一致。該步驟有利於機器人在行進方向與目標路徑方向差異較大時，即在需要進行較大角度轉彎的地方，快速的調整其自身的行進方向；
步驟4.3：移動機器人直線行走過程中，通過雷射傳感器不斷地確定自身的定位信息，實時計算機器人中心點PR(xR，yR)到目標路徑的垂直距離Δs、機器人行進方向與目標路徑的方向差Δθ以及中心點到目標節點P』(xp，yp)的距離d。中心點到目標路徑的距離計算公式如下：其中，公式中目標路徑所在的直線路徑方程為Ax+By+C＝0；中心點到目標節點的距離公式如下：
步驟4.4：移動機器人可設定一定時間調整一次左右輪速，已調整行進方向。移動機器人的輪速控制量計算方法為：ΔV＝ks·Δs+kθ·Δθ，其中ks,kθ分別為預設的距離Δs和方向差Δθ的權值，其具體取值可通過試驗進行不斷優化。兩者的取值不一定要採用定值，可以在不同的情況下採用不同的權重，比如根據優先調整的參數，先設置其權值較大，當該參數被調整至目標範圍內時，在使另外一個參數的權值較大，對其進行調整。根據前一時刻的左輪輪速VL-before和右輪輪速VR-before，可以計算當前時刻的左輪輪速VL-after＝VL-before+ΔV，當前時刻的右輪輪速VR-after＝VR-before-ΔV。然後利用這兩個計算得到的輪速，實時更新並控制移動機器人的左輪和右輪，實現通過左右輪差速調整機器人的前進方向。
步驟5：使移動機器人準確停靠在路徑上的目標停靠點，具體子步驟：
步驟5.1：當移動機器人運動至目標節點附近時，需對其進行減速控制。判斷步驟4.3中移動機器人中心與目標節點的距離d是否小於一定閾值d0，若d小於d0，則視為移動機器人已經到達目標節點附近；
步驟5.2：然後控制移動機器人進行減速，具體方法為：對前面計算得到的左右輪速度增加一個動態控制量kd(0≤kd≤1)，使VL-after』＝kd·VL-after，VR-after』＝kd·VR-after。然後分別利用VL-after』和VR-after』控制左輪和右輪輪速。當移動機器人不斷靠近目標節點，即d越小時kd值也越小直至0，最終使移動機器人在目標節點處停下。
步驟5.3：到達一個節點後，將步驟3中得到的最短路徑中的下一個節點P2和下一條路徑L2分別作為目標節點和目標路徑，重複步驟4.2至步驟5.2。依次類推，直至運動至路徑Ln，並停靠在目標停靠點Ps。