本發(fā)明屬于智能機(jī)器人導(dǎo)航領(lǐng)域，特別地涉及一種基于激光數(shù)據(jù)的移動(dòng)機(jī)器人導(dǎo)航控制方法。

背景技術(shù)：

在機(jī)器人研究領(lǐng)域中，機(jī)器人導(dǎo)航技術(shù)快速發(fā)展。隨著時(shí)代的進(jìn)步以及科技水平的不斷提高，在環(huán)境中對機(jī)器人進(jìn)行準(zhǔn)確的定位也變得越來越重要，導(dǎo)航包含了多種不同的技術(shù)和應(yīng)用：數(shù)據(jù)關(guān)聯(lián)、映射算法、定位算法和同步定位與地圖構(gòu)建等。每一個(gè)步驟和細(xì)節(jié)可以通過不同的方法或傳感器獲取。

移動(dòng)機(jī)器人導(dǎo)航方法主要可以分為兩種：一種是基于地圖的導(dǎo)航方法，另一種是無地圖導(dǎo)航方法。基于地圖的導(dǎo)航方法主要是通過自動(dòng)或者半自動(dòng)方法構(gòu)建地圖，通常采用激光測距儀、超聲波傳感器、或視覺系統(tǒng)來構(gòu)建地圖。地圖主要有四種：柵格地圖、拓?fù)涞貓D、特征地圖以及混合地圖。柵格地圖通常是由激光測距儀產(chǎn)生的，有些方法使用超聲波傳感器也可以得到類似的柵格地圖。拓?fù)涞貓D的構(gòu)建也是使用相同的傳感器但是地圖的思想不一樣。機(jī)器人在特定的位置記錄路標(biāo)，使用路標(biāo)之間的關(guān)聯(lián)來對機(jī)器人進(jìn)行定位。大多數(shù)的特征地圖的構(gòu)建是通過照相機(jī)獲取然后用計(jì)算機(jī)視覺算法處理得到。然后，機(jī)器人通過對觀測值和期望值之間的比較對自身進(jìn)行定位。如果映射方法能夠綜合以上兩種或三種方法或傳感器，稱之為混合地圖。

目前正在運(yùn)行的移動(dòng)機(jī)器人主要是采用磁軌跡規(guī)劃巡檢路線，以及無線射頻識(shí)別(RFID)等技術(shù)實(shí)現(xiàn)導(dǎo)航。這種方法需預(yù)先設(shè)定巡檢機(jī)器人的巡檢路線，然后在運(yùn)行路線上鋪設(shè)磁軌道，并且在機(jī)器人停靠的位置預(yù)先設(shè)置RFID標(biāo)識(shí)，存在地面施工復(fù)雜工作量大、機(jī)器人運(yùn)行路線不靈活等問題。而采用二維激光導(dǎo)航，不僅定位準(zhǔn)確度高、成本低、不受光線影響，而且具備抗電磁干擾，對配置系統(tǒng)要求不高等優(yōu)點(diǎn)。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于解決現(xiàn)有技術(shù)中存在的問題，并提供一種基于激光數(shù)據(jù)的移動(dòng)機(jī)器人導(dǎo)航控制方法。本發(fā)明所采用的具體技術(shù)方案如下:

基于激光數(shù)據(jù)的移動(dòng)機(jī)器人導(dǎo)航控制方法，包括如下步驟：

步驟1：在全局地圖中添加、設(shè)置路徑和停靠點(diǎn)；

步驟2：獲取移動(dòng)機(jī)器人的實(shí)時(shí)定位信息；

步驟3：規(guī)劃移動(dòng)機(jī)器人導(dǎo)航所需最短路徑；

步驟4：移動(dòng)機(jī)器人沿所規(guī)劃路徑導(dǎo)航行駛，且在行駛過程中，實(shí)時(shí)計(jì)算機(jī)器人中心點(diǎn)到目標(biāo)路徑的距離、與目標(biāo)路徑的方向差以及中心點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離，并實(shí)時(shí)調(diào)整機(jī)器人的兩側(cè)輪速，使其逐漸靠近目標(biāo)節(jié)點(diǎn)；

步驟5：當(dāng)?shù)竭_(dá)目標(biāo)節(jié)點(diǎn)附近時(shí)，進(jìn)行減速，使移動(dòng)機(jī)器人停靠在目標(biāo)節(jié)點(diǎn)附近；依次對后續(xù)的目標(biāo)節(jié)點(diǎn)重復(fù)上述步驟，使移動(dòng)機(jī)器人最終停靠在目標(biāo)停靠點(diǎn)上。

作為優(yōu)選，所述步驟1的具體過程為：

步驟1.1：進(jìn)行地圖路徑設(shè)置，在點(diǎn)云地圖中添加節(jié)點(diǎn)，同時(shí)連接節(jié)點(diǎn)繪制路徑；

步驟1.2：選擇一條已繪制的路徑，并在此路徑上添加點(diǎn)作為移動(dòng)機(jī)器人導(dǎo)航時(shí)的一個(gè)停靠點(diǎn)；

步驟1.3：將各路徑的起始節(jié)點(diǎn)坐標(biāo)、終止節(jié)點(diǎn)坐標(biāo)、路徑長度、路徑方向信息存儲(chǔ)至數(shù)據(jù)庫中，同時(shí)也將停靠點(diǎn)的編號信息、坐標(biāo)信息存儲(chǔ)至數(shù)據(jù)庫中。

作為優(yōu)選，所述步驟2的具體過程為：

步驟2.1：移動(dòng)機(jī)器人自帶的激光傳感器以一定的頻率和角度范圍平面掃描外部環(huán)境，獲取激光傳感器所在位置與周邊環(huán)境間的距離數(shù)據(jù)，構(gòu)成此次掃描的點(diǎn)云數(shù)據(jù)；

步驟2.2：移動(dòng)機(jī)器人初始定位時(shí)，預(yù)設(shè)一個(gè)初始位置值作為迭代匹配算法的估計(jì)配準(zhǔn)量，非初始定位時(shí)將前一幀點(diǎn)云數(shù)據(jù)匹配所得的定位值作為后一幀激光數(shù)據(jù)用于匹配的估計(jì)配準(zhǔn)量；

步驟2.3：通過迭代匹配算法將掃描所得點(diǎn)云數(shù)據(jù)與現(xiàn)有全局地圖的整體點(diǎn)云數(shù)據(jù)進(jìn)行匹配，獲得移動(dòng)機(jī)器人上的激光傳感器在全局地圖坐標(biāo)系下的定位信息，包括X、Y軸坐標(biāo)值(x_L，y_L)和方向角θ_L；

步驟2.4：將獲得的激光傳感器的定位信息轉(zhuǎn)化為移動(dòng)機(jī)器人中心點(diǎn)P_R的坐標(biāo)值(x_R，y_R)和方向角θ_R，并以中心點(diǎn)的定位值作為整個(gè)移動(dòng)機(jī)器人的定位信息；

其中，x_R＝x_L-L·cosθ_L，y_R＝y(tǒng)_L-L·sinθ_L，θ_R＝θ_L,其中L為激光傳感器到移動(dòng)機(jī)器人中心點(diǎn)的平面距離。

迭代匹配算法可選擇現(xiàn)有技術(shù)中的方法，例如ICP算法，就是一種典型的迭代匹配算法。該算法是一種通過在迭代過程中不斷降低配準(zhǔn)誤差來獲取采樣點(diǎn)相對位置變換的掃描匹配方法。主要分為兩大部分，對應(yīng)點(diǎn)的搜索和變換參數(shù)的求解。整個(gè)算法就是在對對應(yīng)點(diǎn)對的搜素——變換參數(shù)的計(jì)算——對應(yīng)點(diǎn)進(jìn)行剛性變換——檢驗(yàn)?zāi)繕?biāo)函數(shù)值的迭代過程中不斷降低匹配誤差以獲得兩組數(shù)據(jù)間的相對位姿變化的最優(yōu)變換參數(shù)(R,T)，其中R為旋轉(zhuǎn)矩陣，T為平移矩陣。

作為優(yōu)選，所述步驟3的具體過程為：

步驟3.1：移動(dòng)機(jī)器人獲取數(shù)據(jù)庫中該目標(biāo)停靠點(diǎn)的信息；

步驟3.2：獲取移動(dòng)機(jī)器人的初始定位信息(x_R0，y_R0，θ_R0)；

步驟3.3：通過最短路徑算法計(jì)算移動(dòng)機(jī)器人的初始位置到目標(biāo)停靠點(diǎn)位置P_s(x_s，y_s)運(yùn)行距離最短的路徑組合；

步驟3.4：存儲(chǔ)步驟3.3中獲得的最短距離的路徑信息，包括所需途經(jīng)的節(jié)點(diǎn)順序(P₁，P₂…P_n-1，P_s)和路徑順序(L₁、L₂…L_n-1，L_n)。

最短路徑算法可采用現(xiàn)有技術(shù)中的算法，例如Dijkstra算法，是典型的最短路徑算法，用于計(jì)算一個(gè)節(jié)點(diǎn)到其他所有節(jié)點(diǎn)的最短路徑。將地圖中規(guī)劃的所有路徑、節(jié)點(diǎn)、停靠點(diǎn)的組合視為帶權(quán)有向圖，每條路徑均為雙向，且各路徑長度為該路徑的權(quán)值。將機(jī)器人初始位置作為源點(diǎn)，通過Dijkstra算法計(jì)算源點(diǎn)到圖中其他各點(diǎn)的最短路徑，而后選出源點(diǎn)到目標(biāo)停靠點(diǎn)的最短路徑。

作為優(yōu)選，所述步驟4的具體過程為：

步驟4.1：移動(dòng)機(jī)器人在導(dǎo)航初始時(shí)刻，將步驟3中得到的最短路徑中的第一個(gè)節(jié)點(diǎn)P₁和第一條路徑L₁分別作為目標(biāo)節(jié)點(diǎn)和目標(biāo)路徑；

步驟4.2：計(jì)算移動(dòng)機(jī)器人當(dāng)前方向角與目標(biāo)路徑方向的角度差Δθ₀，移動(dòng)機(jī)器人以角度Δθ₀進(jìn)行原地旋轉(zhuǎn)，調(diào)整行進(jìn)方向與目標(biāo)路徑方向保持一致；

步驟4.3：移動(dòng)機(jī)器人向前行進(jìn)，在行進(jìn)過程中實(shí)時(shí)計(jì)算機(jī)器人中心點(diǎn)P_R(x_R，y_R)到目標(biāo)路徑的垂直距離Δs、機(jī)器人行進(jìn)方向與目標(biāo)路徑的方向差Δθ以及機(jī)器人中心點(diǎn)到目標(biāo)節(jié)點(diǎn)的直線距離d；例如，中心點(diǎn)到目標(biāo)路徑的距離計(jì)算公式如下：其中，公式中目標(biāo)路徑所在的直線路徑方程為Ax+By+C＝0；中心點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離公式如下：

步驟4.4：計(jì)算移動(dòng)機(jī)器人輪速控制量ΔV＝k_s·Δs+k_θ·Δθ，其中k_s,k_θ分別為預(yù)設(shè)的距離Δs和方向差Δθ的權(quán)值；從而根據(jù)前一時(shí)刻的左輪輪速V_L-before和右輪輪速V_R-before，實(shí)時(shí)更新并控制移動(dòng)機(jī)器人當(dāng)前時(shí)刻的左輪輪速V_L-after和右輪輪速V_R-after，其中V_L-after＝V_L-before+ΔV，V_R-after＝V_R-before-ΔV。

作為優(yōu)選，所述步驟5的具體過程為：

步驟5.1：在移動(dòng)機(jī)器人行進(jìn)過程中，判斷機(jī)器人中心與目標(biāo)節(jié)點(diǎn)的距離d是否小于一定閾值d₀，若d小于d₀，則視為移動(dòng)機(jī)器人已經(jīng)到達(dá)目標(biāo)節(jié)點(diǎn)附近；

步驟5.2：然后控制移動(dòng)機(jī)器人進(jìn)行減速，對左右輪速度增加動(dòng)態(tài)控制量控制左輪輪速為V’_L-after和右輪輪速為V’_R-after，其中V_L-after’＝k_d·V_L-after，V_R-after’＝k_d·V_R-after；當(dāng)移動(dòng)機(jī)器人不斷靠近目標(biāo)節(jié)點(diǎn)，即d越小時(shí)k_d值也越小直至0，使移動(dòng)機(jī)器人在目標(biāo)節(jié)點(diǎn)處停下；

步驟5.3：將步驟3中得到的最短路徑中的下一個(gè)節(jié)點(diǎn)P₂和下一條路徑L₂分別作為目標(biāo)節(jié)點(diǎn)和目標(biāo)路徑，重復(fù)步驟4.2至步驟5.2；依次類推，直至運(yùn)動(dòng)至路徑L_n，并停靠在目標(biāo)停靠點(diǎn)P_s。

本發(fā)明數(shù)據(jù)采集操作簡單，且無需預(yù)先了解環(huán)境結(jié)構(gòu)空間，也無需額外鋪設(shè)地標(biāo)，成本低廉、地圖創(chuàng)建速度快精度高，可實(shí)現(xiàn)移動(dòng)機(jī)器人在室內(nèi)室外環(huán)境下的準(zhǔn)確導(dǎo)航控制。其他技術(shù)效果將在具體實(shí)施方式中進(jìn)一步說明。

附圖說明

圖1全局地圖路徑規(guī)劃；

圖2移動(dòng)機(jī)器人導(dǎo)航時(shí)各控制量示意圖。

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)施方式對本發(fā)明做進(jìn)一步闡述，以便更好的理解本發(fā)明。本發(fā)明的技術(shù)特征在沒有沖突的情況下，均可進(jìn)行相互組合。

本實(shí)施方式中，移動(dòng)機(jī)器人并沒有構(gòu)造和型號上的限制，但其用于驅(qū)動(dòng)的左輪和右輪需要能夠分別調(diào)整輪速。其正前端搭載用于掃描周邊環(huán)境的激光傳感器，激光傳感器中心與移動(dòng)機(jī)器人中心點(diǎn)的連線與機(jī)器人的行進(jìn)方向一致。

基于激光數(shù)據(jù)的移動(dòng)機(jī)器人導(dǎo)航控制方法，包括如下步驟和子步驟：

步驟1：全局地圖中添加和設(shè)置路徑，具體子步驟：

步驟1.1：首先構(gòu)建一個(gè)用于控制機(jī)器人和輸入指令、信息的客戶端，通過客戶端進(jìn)行地圖路徑設(shè)置，在點(diǎn)云地圖中添加節(jié)點(diǎn)，同時(shí)連接節(jié)點(diǎn)繪制路徑。本實(shí)施例中地圖路徑節(jié)點(diǎn)如圖1所示。

步驟1.2：通過客戶端添加停靠點(diǎn)，選擇一條已繪制的路徑，并在此路徑上添加點(diǎn)作為移動(dòng)機(jī)器人導(dǎo)航時(shí)的一個(gè)停靠點(diǎn)，該停靠點(diǎn)將作為最后一個(gè)目標(biāo)節(jié)點(diǎn)。

步驟1.3：將各路徑的起始節(jié)點(diǎn)坐標(biāo)、終止節(jié)點(diǎn)坐標(biāo)、路徑長度、路徑方向信息存儲(chǔ)至數(shù)據(jù)庫中，同時(shí)也將停靠點(diǎn)的編號信息、坐標(biāo)信息存儲(chǔ)至數(shù)據(jù)庫中，供機(jī)器人的控制系統(tǒng)調(diào)用。

步驟2：獲取移動(dòng)機(jī)器人的實(shí)時(shí)定位信息，具體子步驟：

步驟2.1：移動(dòng)機(jī)器人自帶的激光傳感器以一定的頻率和角度范圍平面掃描外部環(huán)境，獲取激光傳感器所在位置與周邊環(huán)境間的距離數(shù)據(jù)，構(gòu)成此次掃描的點(diǎn)云數(shù)據(jù)；

步驟2.2：移動(dòng)機(jī)器人第一次開始定位時(shí)，為了提高定位的速度和準(zhǔn)確性，可事先由用戶估計(jì)一個(gè)初始位置值，并輸入客戶端，然后將初始位置值作為迭代匹配算法的估計(jì)配準(zhǔn)量。當(dāng)非初始定位時(shí)，可以將前一幀點(diǎn)云數(shù)據(jù)匹配所得的定位值作為后一幀激光數(shù)據(jù)用于匹配的估計(jì)配準(zhǔn)量；

步驟2.3：通過迭代匹配算法將掃描所得點(diǎn)云數(shù)據(jù)與現(xiàn)有全局地圖的整體點(diǎn)云數(shù)據(jù)進(jìn)行匹配，獲得激光傳感器在全局地圖坐標(biāo)系下的定位信息，包括X、Y軸坐標(biāo)值(x_L，y_L)和方向角θ_L(機(jī)器人行進(jìn)方向與X軸的夾角)。在機(jī)器人后續(xù)的行進(jìn)過程中，也通過該方法不斷地確定自身的定位信息，以計(jì)算各種參數(shù)。

本實(shí)施例中采用ICP算法作為迭代匹配算法。該算法的具體實(shí)現(xiàn)過程如下：

給定激光數(shù)據(jù)點(diǎn)集P＝{P_i,i＝0,1,2,…,m}及全局地圖數(shù)據(jù)點(diǎn)集Q＝{Q_i,i＝0,1,2,…,n}，設(shè)置最小誤差度量值ε和最大迭代次數(shù)D。

(1)估計(jì)配準(zhǔn)：在數(shù)據(jù)點(diǎn)集P與Q匹配之前，將前一次迭代匹配完成后獲得的匹配結(jié)果作為P與Q的估計(jì)配準(zhǔn)值。沒有估計(jì)配準(zhǔn)，匹配算法會(huì)陷入局部極小值。

(2)尋找對應(yīng)點(diǎn)：根據(jù)歐式距離最小原則尋找，歐式距離定義如下：

(3)對于集合Q中的各點(diǎn)，在集合P中找出距該點(diǎn)最近的對應(yīng)點(diǎn)，且設(shè)集合P中由這些對應(yīng)點(diǎn)組成的新點(diǎn)集為P‘＝{P_i‘,i＝0,1,2,…,n}。作為求解變換的匹配點(diǎn)。

(4)尋找變換(R,T)：使得兩組數(shù)據(jù)點(diǎn)間的誤差度量值最小，計(jì)算公式如下：

其中ω為旋轉(zhuǎn)弧度，旋轉(zhuǎn)矩陣平移向量Q_i為參考點(diǎn)集，P_i’為對應(yīng)的帶匹配的點(diǎn)集。通過最小化E_k，可以得到T_x、T_y和ω的解。

(5)應(yīng)用變換，更新機(jī)器人位姿。用配準(zhǔn)變換矩陣R、T進(jìn)行坐標(biāo)變換，得到新的點(diǎn)集Q₁，即Q₁＝RQ+T，計(jì)算E_k+1。

(6)迭代：當(dāng)|E_k-E_k+1|>ε，重復(fù)步驟(3)-(5)。否則，跳出迭代，計(jì)算結(jié)束，匹配成功。

步驟2.4：由于激光傳感器測量得到的定位信息實(shí)際上是其自身的定位，但其與移動(dòng)機(jī)器人的中心點(diǎn)存在一定的位置差異，因此需要進(jìn)行校正：首先測量激光傳感器到移動(dòng)機(jī)器人中心點(diǎn)的平面距離L，將步驟2.3中獲得的激光傳感器的定位信息轉(zhuǎn)化為移動(dòng)機(jī)器人中心點(diǎn)P_R的坐標(biāo)值(x_R，y_R)和方向角(θ_R)，并以中心點(diǎn)的定位值作為整個(gè)移動(dòng)機(jī)器人的定位信息。

其中，x_R＝x_L-L·cosθ_L，y_R＝y(tǒng)_L-L·sinθ_L，θ_R＝θ_L。

步驟3：規(guī)劃移動(dòng)機(jī)器人導(dǎo)航所需最短路徑。具體子步驟：

步驟3.1：通過客戶端確定目標(biāo)停靠點(diǎn)，移動(dòng)機(jī)器人獲取數(shù)據(jù)庫中該目標(biāo)停靠點(diǎn)的信息；

步驟3.2：通過步驟2獲取移動(dòng)機(jī)器人的初始定位信息(x_R0，_yR0，θ_R0)；

步驟3.3：通過最短路徑算法計(jì)算移動(dòng)機(jī)器人初始位置到目標(biāo)停靠點(diǎn)位置P_s(x_s，_ys)運(yùn)行距離最短的路徑；

Dijkstra算法是典型的最短路徑算法，該算法的具體實(shí)現(xiàn)過程如下：

(1)算法思想：設(shè)G＝(V,E)是一個(gè)帶權(quán)有向圖，把圖中頂點(diǎn)集合V分成兩組，第一組為已求出最短路徑的頂點(diǎn)集合(用S表示，初始時(shí)S中只有一個(gè)源點(diǎn)，以后每求得一條最短路徑,就將加入到集合S中，直到全部頂點(diǎn)都加入到S中，算法就結(jié)束了)，第二組為其余未確定最短路徑的頂點(diǎn)集合(用U表示)，按最短路徑長度的遞增次序依次把第二組的頂點(diǎn)加入S中。在加入的過程中，總保持從源點(diǎn)_v到S中各頂點(diǎn)的最短路徑長度不大于從源點(diǎn)v到U中任何頂點(diǎn)的最短路徑長度。此外，每個(gè)頂點(diǎn)對應(yīng)一個(gè)距離，S中的頂點(diǎn)的距離就是從v到此頂點(diǎn)的最短路徑長度，U中的頂點(diǎn)的距離，是從v到此頂點(diǎn)只包括S中的頂點(diǎn)為中間頂點(diǎn)的當(dāng)前最短路徑長度。

(2)算法步驟：

a.初始時(shí)，S只包含源點(diǎn)，即S＝{v}，v的距離為0。U包含除v外的其他頂點(diǎn)，即:U＝{其余頂點(diǎn)}，若v與U中頂點(diǎn)u有邊，則<u,v>正常有權(quán)值，若u不是v的出邊鄰接點(diǎn)，則<u,v>權(quán)值為∞。

b.從U中選取一個(gè)距離v最小的頂點(diǎn)m，把m，加入S中(該選定的距離就是v到m的最短路徑長度)。

c.以m為新考慮的中間點(diǎn)，修改U中各頂點(diǎn)的距離；若從源點(diǎn)v到頂點(diǎn)u的距離(經(jīng)過頂點(diǎn)m)比原來距離(不經(jīng)過頂點(diǎn)m)短，則修改頂點(diǎn)u的距離值，修改后的距離值的頂點(diǎn)m的距離加上邊上的權(quán)。

d.重復(fù)步驟b和c直到所有頂點(diǎn)都包含在S中。

步驟3.4：存儲(chǔ)步驟3.3中獲得的最短距離的路徑信息，包括所需途經(jīng)的節(jié)點(diǎn)順序(P₁，P₂…P_n-1，P_s)和路徑順序(L₁、L₂…L_n-1，L_n)，P_s表示第s個(gè)節(jié)點(diǎn)，L_n表示第n個(gè)條路徑。

步驟4：移動(dòng)機(jī)器人沿所規(guī)劃路徑導(dǎo)航行駛。如圖2，具體子步驟：

步驟4.1：移動(dòng)機(jī)器人在導(dǎo)航初始時(shí)刻，將步驟3中得到的最短路徑中的第一個(gè)節(jié)點(diǎn)P₁和第一條路徑L₁分別作為目標(biāo)節(jié)點(diǎn)和目標(biāo)路徑，機(jī)器人第一步的目的是移動(dòng)到該目標(biāo)節(jié)點(diǎn)；

步驟4.2：計(jì)算移動(dòng)機(jī)器人當(dāng)前方向角與目標(biāo)路徑方向的角度差Δθ₀，移動(dòng)機(jī)器人以角度Δθ₀進(jìn)行原地旋轉(zhuǎn)，調(diào)整自身方向與目標(biāo)路徑方向保持一致。該步驟有利于機(jī)器人在行進(jìn)方向與目標(biāo)路徑方向差異較大時(shí)，即在需要進(jìn)行較大角度轉(zhuǎn)彎的地方，快速的調(diào)整其自身的行進(jìn)方向；

步驟4.3：移動(dòng)機(jī)器人直線行走過程中，通過激光傳感器不斷地確定自身的定位信息，實(shí)時(shí)計(jì)算機(jī)器人中心點(diǎn)P_R(x_R，y_R)到目標(biāo)路徑的垂直距離Δs、機(jī)器人行進(jìn)方向與目標(biāo)路徑的方向差Δθ以及中心點(diǎn)到目標(biāo)節(jié)點(diǎn)P’(x_p，y_p)的距離d。中心點(diǎn)到目標(biāo)路徑的距離計(jì)算公式如下：其中，公式中目標(biāo)路徑所在的直線路徑方程為Ax+By+C＝0；中心點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離公式如下：

步驟4.4：移動(dòng)機(jī)器人可設(shè)定一定時(shí)間調(diào)整一次左右輪速，已調(diào)整行進(jìn)方向。移動(dòng)機(jī)器人的輪速控制量計(jì)算方法為：ΔV＝k_s·Δs+k_θ·Δθ，其中k_s,k_θ分別為預(yù)設(shè)的距離Δs和方向差Δθ的權(quán)值，其具體取值可通過試驗(yàn)進(jìn)行不斷優(yōu)化。兩者的取值不一定要采用定值，可以在不同的情況下采用不同的權(quán)重，比如根據(jù)優(yōu)先調(diào)整的參數(shù)，先設(shè)置其權(quán)值較大，當(dāng)該參數(shù)被調(diào)整至目標(biāo)范圍內(nèi)時(shí)，在使另外一個(gè)參數(shù)的權(quán)值較大，對其進(jìn)行調(diào)整。根據(jù)前一時(shí)刻的左輪輪速V_L-before和右輪輪速V_R-before，可以計(jì)算當(dāng)前時(shí)刻的左輪輪速V_L-after＝V_L-before+ΔV，當(dāng)前時(shí)刻的右輪輪速V_R-after＝V_R-before-ΔV。然后利用這兩個(gè)計(jì)算得到的輪速，實(shí)時(shí)更新并控制移動(dòng)機(jī)器人的左輪和右輪，實(shí)現(xiàn)通過左右輪差速調(diào)整機(jī)器人的前進(jìn)方向。

步驟5：使移動(dòng)機(jī)器人準(zhǔn)確停靠在路徑上的目標(biāo)停靠點(diǎn)，具體子步驟：

步驟5.1：當(dāng)移動(dòng)機(jī)器人運(yùn)動(dòng)至目標(biāo)節(jié)點(diǎn)附近時(shí)，需對其進(jìn)行減速控制。判斷步驟4.3中移動(dòng)機(jī)器人中心與目標(biāo)節(jié)點(diǎn)的距離d是否小于一定閾值d₀，若d小于d₀，則視為移動(dòng)機(jī)器人已經(jīng)到達(dá)目標(biāo)節(jié)點(diǎn)附近；

步驟5.2：然后控制移動(dòng)機(jī)器人進(jìn)行減速，具體方法為：對前面計(jì)算得到的左右輪速度增加一個(gè)動(dòng)態(tài)控制量k_d(0≤k_d≤1)，使V_L-after’＝k_d·V_L-after，V_R-after’＝k_d·V_R-after。然后分別利用V_L-after’和V_R-after’控制左輪和右輪輪速。當(dāng)移動(dòng)機(jī)器人不斷靠近目標(biāo)節(jié)點(diǎn)，即d越小時(shí)k_d值也越小直至0，最終使移動(dòng)機(jī)器人在目標(biāo)節(jié)點(diǎn)處停下。

步驟5.3：到達(dá)一個(gè)節(jié)點(diǎn)后，將步驟3中得到的最短路徑中的下一個(gè)節(jié)點(diǎn)P₂和下一條路徑L₂分別作為目標(biāo)節(jié)點(diǎn)和目標(biāo)路徑，重復(fù)步驟4.2至步驟5.2。依次類推，直至運(yùn)動(dòng)至路徑L_n，并停靠在目標(biāo)停靠點(diǎn)P_s。