本發明涉及機器人控制的技術領域，尤其是指一種基于ros機器人操作系統的機器人自動跟隨方法。

背景技術：

近些年來，機器人技術發展迅速，取得了驚人的成果，越來越多的機器人代替人類在各個領域發揮了不可替代的作用。尤其是人工智能(ai)時代的到來，許多新型機器人應運而生。例如家居服務機器人，導購機器人，掃地機器人等。機器人技術已經成為推動人類進步發展的重要技術之一。機器人自動跟隨功能是機器人技術的一個重要方面，在許多應用場合都需要具備自動跟隨功能。例如家居服務機器人，導購機器人等。因此，對機器人自動跟隨技術進行研究，具有非常重要的意義。

目前實現機器人自動跟隨方法有很多，其中應用最多的是基于視覺的機器人自動跟隨技術。通過攝像機識別行人目標，從而實現自動跟隨?；谝曈X的行人檢測，受光照、攝像機視野范圍的影響，在某些應用場合并不能發揮理想的效果。另一種是基于激光雷達的自動跟隨技術，激光雷達能夠較精確的獲得周圍環境的距離信息，且不受光照等環境因素的影響，但由于激光雷達只能采集行人的距離信息，特征并不明顯，所以對算法的要求較高，往往對行人的檢測精度并不是很高，所以需要對行人檢測以及跟隨算法進行優化。

綜合以上論述，發明一種精度較高、適用環境范圍廣的機器人自動跟隨方法具有較高的實際應用價值。

技術實現要素：

本發明的目的在于克服傳統的基于視覺和基于激光雷達的機器人自動跟隨技術的不足，提出了一種基于ros機器人操作系統的機器人自動跟隨方法，采用一定時間內進行重采樣的方法，解決激光雷達特征不足，行人識別準確率較低的缺點，并利用ros機器人操作系統進行實現，合理利用了ros的navigation框架，使機器人在自動跟隨過程中具備一定的路徑規劃和避障能力。

為實現上述目的，本發明所提供的技術方案為：一種基于ros機器人操作系統的機器人自動跟隨方法，包括以下步驟：

1)傳感器數據處理：使用激光雷達采集數據，對數據進行預處理，剔除那些超出預設范圍的數據，并將結果轉化到全局坐標系；

2)數據聚類：使用層次聚類算法對數據進行聚類，去除噪聲點并對環境進行分割；

3)行人識別：對聚類后的結果進行行人識別，以類圓弧算法識別行人腿部模型，以行人雙腿特征作為行人檢測特征，識別出可能的行人位置；

4)重采樣：在設定時間內對行人重新進行檢測，根據前后兩次檢測結果，排除干擾項，鎖定行人目標，并綜合利用機器人當前位置和行人位置，確定下一時刻目標位置，避免機器人與行人碰撞；

5)導航：根據行人目標確定導航目標，使用a*算法規劃出全局路徑，并使用dwa動態窗口算法規劃局部路徑，控制機器人朝導航目標運動，并實時避開障礙物。

在步驟1)中，使用激光雷達采集數據，對數據進行預處理，將數據限制在[min_size,max_size]范圍內：

min_size≤data_useful≤max_size

其中，min_size、max_size分別代表最小和最大的激光雷達數據，data_useful代表有效數據；

利用ros的tf框架，將采集的數據從激光雷達坐標系tf/laser轉化到全局坐標系tf/base_link，scan代表在激光雷達坐標系tf/laser下的數據類型，detect_data代表在全局坐標系tf/base_link下的數據類型：

在步驟2)中，使用層次聚類算法進行聚類的步驟如下：

2.1)將detect_data的每一個元素當成一個類，計算相鄰每個類的最小距離；

2.2)將距離小于lei_size的兩個類合并為一個類，lei_size表示可以合并成一類的兩個數據的最小距離；

2.3)重新計算新類與所有類之間的距離；

2.4)重復步驟2.2)、2.3)，直到所有類不能再合并為止；

根據每個類的元素個數對所有類進行選擇，保留元素個數在[num_min,num_max]之間的類，從而分割出聚類點數多的環境墻體，以及聚類點數少的噪聲點，num_min、num_max分別代表最小和最大元素個數。

在步驟3)中，以類圓弧算法識別行人腿部模型，以行人雙腿模型作為行人識別特征：

3.1)計算每個類的總長度lk和首尾元素的直線距離dk：

其中classk[n]代表聚類后的結果，k表示類的標號，n表示一個類中元素的數量，i代表一個類中元素的標號，classk[i].x、classk[i].y分別代表第k個類的第i個元素的橫坐標和縱坐標；

3.2)計算每個類的彎曲度curve[k]，k表示類的標號:

3.3)判斷curve[k]＞δc，且lmin＜lk＜lmax，則把該類當成候選leg簇，δc表示最小的曲率，lmin、lmax分別代表最小和最大的類長度；

3.4)計算所有候選leg簇的中心坐標：

其中xc和yc分別代表第k個類的中心的橫坐標和縱坐標，將所有類的中心坐標記為leg_array[n](xc,yc)，n代表類的數量；

3.5)計算leg_array[n]中每兩個leg之間的距離dist_leg：

其中，leg_array[k].x、leg_array[k].y和leg_array[k+1].x、leg_array[k+1].y分別代表第k個leg和第k+1個leg位置的橫坐標和縱坐標，若dist_leg[k]＜leg_dist，leg_dist表示允許的最大行人雙腿間距，則計算leg_array[k]和leg_array[k+1]的中間位置，作為可能的行人坐標：

xp＝(leg_array[k].x+leg_array[k+1].x)/2

yp＝(leg_array[k].y+leg_array[k+1].y)/2。

xp和yp分別代表可能的行人位置的橫坐標和縱坐標，將所有可能的行人位置記為people_array[n](xp,yp)，n代表可能的行人數量；

在步驟4)中，是在dt時間內重新執行步驟1)～3)將得到的結果記為people_array_new[n](xp,yp)，n代表可能的行人數量,并進行以下步驟：

4.1)比較people_array(xp,yp)和people_array_new(xp,yp)相對應的每一個元素，計算相對應元素之間的距離dist_people[k]，k代表元素標號：

其中people_array[k].xp、people_array[k].yp和people_array_new[k].xp、people_array_new[k].yp分別代表重采樣之前和重采樣后的第k個可能的行人位置的橫坐標和縱坐標；

4.2)將min_dist≤dist_people[k]≤max_dist，對應的people_array_new[k]確定為最終的行人位置people_pos(x,y)，min_dist、max_dist分別代表最小和最大的行人移動距離；

4.3)利用機器人電機編碼器的數據，計算出機器人在全局坐標系下的當前位置robot_pos(x,y)；

4.4)計算robot_pos(x,y)到people_pos(x,y)的參數：

其中，robot_pos.x、robot_pos.y和people_pos.x、people_pos.y分別代表機器人位置和行人位置的橫坐標和縱坐標，l表示機器人當前位置robot_pos(x,y)與行人位置people_pos(x,y)的直線距離，θ表示機器人當前位置與行人位置的連線與橫坐標的夾角；

4.5)以save_dist作為機器人跟隨行人運動時的安全間距，計算最終的導航目標navigation_goal(x,y)：

navigation_goal.x＝robot_pos.x+(l-save_dist)·cosθ

navigation_goal.y＝robot_pos.y+(l-save_dist)·sinθ

navigation_goal.x和navigation_goal.y分別代表導航目標的橫坐標和縱坐標；

4.6)以navigation_goal(x,y)為消息類型，發布move_base/goal話題，發布導航目標。

在步驟5)中，利用ros機器人操作系統提供的move_base導航框架，實現機器人的導航功能。move_base是ros機器人操作系統提供的一個功能包，只要給定機器人的導航目標，輸入激光雷達采集的數據、環境地圖，通過設置導航參數，調用導航框架提供的全局路徑規劃和局部路徑規劃模塊，即可實現機器人的自主導航。這里通過訂閱move_base/goal話題，獲得步驟4)發布的導航目標，通過訂閱laser_scan話題獲得激光雷達數據消息scan，設置導航參數選用a*最短路徑算法規劃出全局路徑，使用dwa動態窗口算法規劃局部路徑，避開障礙物，實現了機器人的導航。

本發明與現有技術相比，具有如下優點與有益效果：

1、使用單個激光雷達采集數據，相對于多激光雷達系統成本較低，且相對于視覺的自動跟隨系統使用范圍廣。

2、使用重采樣的方法，比較兩次采樣行人識別之后的結果，有效的解決了單個激光雷達特征較少，行人識別精度低的缺點。

3、具備導航模塊，在機器人自動跟隨過程中，能夠根據環境信息，制定最短路徑，并實時避開障礙物。

4、使用ros機器人操作系統進行實現，合理利用了ros機器人操作系統的節點、消息、話題機制，方便各部分之間的消息傳遞和功能實現。

附圖說明

圖1為本發明的系統框圖。

圖2為本發明的行人識別示意圖。

圖3為本發明的重采樣鎖定行人目標的示意圖。

圖4為本發明根據行人目標確定導航目標的示意圖。

具體實施方式

下面結合具體實施例對本發明做進一步的說明。

本實施例所提供的基于ros機器人操作系統的機器人自動跟隨方法，包括以下步驟：

1)傳感器數據處理：使用激光雷達采集數據，對數據進行預處理，剔除那些超出預設范圍的數據，將數據限制在[min_size,max_size]范圍內：

min_size≤data_useful≤max_size

其中，min_size、max_size分別代表最小和最大的激光雷達數據，data_useful代表有效數據；

利用ros的tf框架，將采集的數據從坐標系tf/laser轉化到全局坐標系tf/base_link，scan代表在激光雷達坐標系tf/laser下的數據類型，detect_data代表在全局坐標系tf/base_link下的數據類型：

2)數據聚類：使用層次聚類算法對數據進行聚類，去除噪聲點并對環境進行分割。其中，使用層次聚類算法進行聚類的步驟如下：

2.1)將detect_data的每一個元素當成一個類，計算相鄰每個類的最小距離；

2.2)將距離小于lei_size的兩個類合并為一個類，lei_size表示可以合并成一類的兩個數據的最小距離，取lei_size＝0.06m；

2.3)重新計算新類與所有類之間的距離；

2.4)重復步驟2.2)、2.3)，直到所有類不能再合并為止；

根據每個類的元素個數對所有類進行選擇，保留元素個數在[num_min,num_max]之間的類，從而分割出聚類點數多的環境墻體，以及聚類點數少的噪聲點，num_min、num_max分別代表最小和最大元素個數。

3)行人識別：對聚類后的結果進行行人識別，以類圓弧算法識別行人腿部模型，以行人雙腿特征作為行人檢測特征，識別出可能的行人位置。其中，以類圓弧算法識別行人腿部模型，以行人雙腿模型作為行人識別特征，包括以下步驟：

3.1)計算每個類的總長度lk和首尾元素的直線距離dk：

其中classk[n]代表聚類后的結果，k表示類的標號，n表示一個類中元素的數量，i代表一個類中元素的標號，classk[i].x、classk[i].y分別代表第k個類的第i個元素的橫坐標和縱坐標；

3.2)計算每個類的彎曲度curve[k]，k表示類的標號:

3.3)判斷curve[k]＞δc，且lmin＜lk＜lmax，則把該類當成候選leg簇，δc表示最小的曲率，lmin、lmax分別代表最小和最大的類長度；

3.4)計算所有候選leg簇的中心坐標：

其中xc和yc分別代表第k個類的中心的橫坐標和縱坐標，將所有類的中心坐標記為leg_array[n](xc,yc)，n代表類的數量；

3.5)計算leg_array[n]中每兩個leg之間的距離dist_leg：

其中，leg_array[k].x、leg_array[k].y和leg_array[k+1].x、leg_array[k+1].y分別代表第k個leg和第k+1個leg位置的橫坐標和縱坐標，若dist_leg[k]＜leg_dist，leg_dist表示允許的最大行人雙腿間距，則計算leg_array[k]和leg_array[k+1]的中間位置，作為可能的行人坐標：

xp＝(leg_array[k].x+leg_array[k+1].x)/2

yp＝(leg_array[k].y+leg_array[k+1].y)/2

xp和yp分別代表可能的行人位置的橫坐標和縱坐標，將所有可能的行人位置記為people_array[n](xp,yp)，n代表可能的行人數量；

4)重采樣：在設定時間內對行人重新進行檢測，根據前后兩次檢測結果，排除干擾項，鎖定行人目標，并綜合利用機器人當前位置和行人位置，確定下一時刻目標位置，避免機器人與行人碰撞。其中在dt時間內重新執行步驟1)～3)將得到的結果記為people_array_new[n](xp,yp)，并進行以下步驟：

4.1)比較people_array(xp,yp)和people_array_new(xp,yp)相對應的每一個元素，計算相對應元素之間的距離dist_people[k]，k代表元素標號：

其中people_array[k].xp、people_array[k].yp和people_array_new[k].xp、people_array_new[k].yp分別代表重采樣之前和重采樣后的第k個可能的行人位置的橫坐標和縱坐標；

4.2)將min_dist≤dist_people[k]≤max_dist，對應的people_array_new[k]確定為最終的行人位置people_pos(x,y)，min_dist、max_dist分別代表最小和最大的行人移動距離；

4.3)利用機器人電機編碼器的數據，計算出機器人在全局坐標系下的當前位置robot_pos(x,y)；

4.4)計算robot_pos(x,y)到people_pos(x,y)的參數：

其中，robot_pos.x、robot_pos.y和people_pos.x、people_pos.y分別代表機器人位置和行人位置的橫坐標和縱坐標，l表示機器人當前位置robot_pos(x,y)與行人位置people_pos(x,y)的直線距離，θ表示機器人當前位置與行人位置的連線與橫坐標的夾角；

4.5)以save_dist作為機器人跟隨行人運動時的安全間距，計算最終的導航目標navigation_goal(x,y)

navigation_goal.x＝robot_pos.x+(l-save_dist)cosθ

navigation_goal.y＝robot_pos.y+(l-save_dist)sinθ

navigation_goal.x和navigation_goal.y分別代表導航目標的橫坐標和縱坐標；

4.6)以navigation_goal(x,y)為消息類型，發布move_base/goaltopic，發布導航目標。

5)導航：根據行人目標確定導航目標，使用a*算法規劃出全局路徑，并使用dwa動態窗口算法規劃局部路徑，控制機器人朝導航目標運動，并實時避開障礙物。具體是：利用ros提供的move_base導航框架，訂閱move_base/goal話題，獲得導航目標，通過訂閱laser_scan話題獲得scan消息，使用a*算法規劃全局路徑，使用dwa動態窗口算法規劃局部路徑，避開障礙物，實現了機器人的導航。

如圖1所示，為本實施例上述方法用到的系統框架，包括robot、激光雷達、數據預處理、行人識別以及navigation五部分。下面分別介紹每部分的具體實現方式：

(1)robot部分：本示例選用的是eai科技的dashgo移動機器人平臺，控制器選用的是一臺裝有ubuntu14.04的筆記本，在linux中裝有rosindigo機器人操作系統。

(2)激光雷達部分：激光雷達作為本系統的數據輸入，分別輸入給行人檢測模塊，以及navigation模塊。選用的是eai科技的flashlidar。能夠360度全方位掃描，掃描頻率為10hz，角度分辨率β＝1°，激光雷達安裝機器人正上方0.3m處，測量有效距離6m。激光雷達的數據通過laser_scantopic發布，消息類型為scan。

(3)數據預處理部分：通過訂閱laser_scantopic，從激光雷達獲得掃描數據，對數據進行預處理，對數據進行閾值控制，只對[min_size,max_size]范圍內的數據進行行人檢測，min_size、max_size分別代表最小和最大的激光雷達數據，data_useful代表有效數據：

min_size≤data_useful≤max_size

取max_size＝5.0m，min_size＝0.5m。由于激光雷達安裝在機器人中心正上方30cm處，scan消息的數據是在激光雷達的安裝位置定義的坐標系tf/laser下，所以將激光雷達的數據轉化到全局坐標系tf/base_link，新消息類型定義為detect_data：

(4)行人識別部分：行人識別部分包括以下三部分：

a、聚類：使用層次聚類算法對數據進行聚類，步驟如下：

1)將detect_data的每一個元素當成一個類，計算相鄰每個類的最小距離；

2)將距離小于lei_size的兩個類合并為一個類，lei_size表示可以合并成一類的兩個數據的最小距離，取lei_size＝0.06m；

3)重新計算新類與所有類之間的距離；

4)重復2)、3)，直到所有類不能再合并為止。

根據每個類的元素個數對所有類進行選擇，保留元素個數在[num_min,num_max]之間的類，從而分割出聚類點數多的環境墻體，以及聚類點數少的噪聲點，num_min、num_max分別代表最小和最大元素個數，取num_min＝5，num_max＝100。

b、行人識別：以類圓弧算法識別行人腿部模型，以行人雙腿模型作為行人識別特征，如圖2所示：

1)計算每個類的總長度lk和首尾元素的直線距離dk：

其中classk[n]代表聚類后的結果，k表示類的標號，n表示一個類中元素的數量，i代表一個類中元素的標號，classk[i].x、classk[i].y分別代表第k個類的第i個元素的橫坐標和縱坐標；

2)計算每個類的彎曲度curve[k]，k表示類的標號:

3)判斷curve[k]＞δc，且lmin＜lk＜lmax，則把該類當成候選leg簇，δc表示最小的曲率，取δc＝1.1，lmin、lmax分別代表最小和最大的類長度，取lmin＝0.1m，lmax＝0.3m；

4)計算所有候選leg簇的中心坐標legk(xk,yk)，將結果記為leg_array[n]中，n代表leg的數量。

其中xc和yc分別代表第k個類的中心的橫坐標和縱坐標，將所有類的中心坐標記為leg_array[n](xc,yc)，n代表類的數量；

5)計算leg_array[n]中每兩個leg之間的距離dist_leg。

其中，leg_array[k].x、leg_array[k].y和leg_array[k+1].x、leg_array[k+1].y分別代表第k個leg和第k+1個leg位置的橫坐標和縱坐標，若dist_leg[k]＜1m，則計算leg_array[k]和leg_array[k+1]的中間位置，作為可能的行人坐標：

xp＝(leg_array[k].x+leg_array[k+1].x)/2

yp＝(leg_array[k].y+leg_array[k+1].y)/2

xp和yp分別代表可能的行人位置的橫坐標和縱坐標，將所有可能的行人位置記為people_array[n](xp,yp)，n代表可能的行人數量；

c、重采樣：在dt時間內重新執行步驟采集數據，對數據進行預處理、聚類、行人識別，將得到的結果記為people_array_new[n](xp,yp)，并進行以下步驟：

1)比較people_array(xp,yp)和people_array_new(xp,yp)相對應的每一個元素，計算相對應元素之間的距離dist_people[k]，k代表元素標號：

其中people_array[k].xp、people_array[k].yp和people_array_new[k].xp、people_array_new[k].yp分別代表重采樣之前和重采樣后的第k個可能的行人位置的橫坐標和縱坐標；

2)將0.1m≤dist_people[k]≤1m對應的people_array_new[k]確定為最終的行人位置，并賦值給people_pos(x,y)。如圖3所示，計算可得dist_people[0]＝0，dist_people[1]＝0.3m，dist_people[2]＝0，故people_pos(x,y)＝people_array_new[1]。

3)利用機器人電機編碼器的數據，計算出機器人在全局坐標系下的當前位置robot_pos(x,y)通，過監聽/odom坐標系到/base_link坐標系的tf變換實現；

4)計算robot_pos(x,y)到people_pos(x,y)的參數，如圖4所示：

其中，robot_pos.x、robot_pos.y和people_pos.x、people_pos.y分別代表機器人位置和行人位置的橫坐標和縱坐標，l表示機器人當前位置robot_pos(x,y)與行人位置people_pos(x,y)的直線距離，θ表示機器人當前位置與行人位置的連線與橫坐標的夾角；

5)以save_dist＝0.5m作為安全距離，計算最終的導航目標navigation_goal(x,y)：

navigation_goal.x＝robot_pos.x+(l-save_dist)cosθ

navigation_goal.y＝robot_pos.y+(l-save_dist)sinθ

navigation_goal.x和navigation_goal.y分別代表導航目標的橫坐標和縱坐標；

6)以navigation_goal(x,y)為消息類型，發布move_base/goaltopic，發布導航目標。

(4)navigation部分：這里使用的是ros機器人操作系統的move_base導航框架，主要包括全局路徑規劃和局部路徑規劃。訂閱move_base/goal話題，獲得下一時刻的目標，使用a*算法規劃出全局路徑，訂閱laser_scan話題，使用dwa算法規劃局部路徑，合理控制機器人速度和加速度，朝目標運動，離目標較遠時增大速度，離目標較近時降低速度，并實時避開障礙物。

以上所述之實施例子只為本發明之較佳實施例，并非以此限制本發明的實施范圍，故凡依本發明之形狀、原理所作的變化，均應涵蓋在本發明的保護范圍內。