面向溺水救援機器人平穩跟蹤的模糊比例微分控制視覺伺服方案

文章編號：1671-3559（2024）02-0227-07DOI：10.13349/j.cnki.jdxbn.20231222.001

摘要： 針對溺水救援機器人的平穩跟蹤需求，提出一種基于模糊比例微分控制的視覺伺服方案，通過場館頂部安裝的俯視攝像頭獲取機器人的坐標位置，使用虛擬導航線設計動態行走路徑，計算偏移方向、 偏移角并制定轉彎規則；利用三角隸屬度函數制定模糊比例、 微分控制規則表，根據2種規則表中的比例、 微分系數分別控制機器人的偏移角、距離偏移量及其變化率，實現機器人的跟蹤與避障的平穩性調整；機器人采用核相關濾波跟蹤算法對溺水人員進行跟蹤，在溺水人員位置變化時調整運動方向，依靠實時、 穩定的聲吶信息設置偏移角，實現避障功能；對所提出的方案在先鋒P3DX型機器人上進行實驗驗證。結果表明：機器人能及時應對目標變化，沒有跟錯、 跟丟現象；在不同工況時的橫滾角均小于1°，避障率達到100%，能夠快速、 穩定、 準確地到達目標位置，達到溺水救援機器人的前期應用要求。

關鍵詞： 溺水救援機器人； 平穩跟蹤； 模糊比例微分控制； 視覺伺服

中圖分類號： TP391.4

文獻標志碼： A

開放科學識別碼（OSID碼）：

Fuzzy Proportional Plus Derivative Control Visual Servo

Scheme for Stationary Tracking of Drowning Rescue Robots

GUO Yinga， b， c， LI Guangweid， LIU Zongshanga， b， c， MOUNZEO Breit Hilleya， b， c， LI Jinpinga， b， c

（a. School of Information Science and Engineering， b. Shandong Provincial Key Laboratory of Network Based Intelligent Computing，

c. Shandong Provincial University Key Laboratory of Information Processing and Cognitive Computing，

d. School of Electrical Engineering， University of Jinan， Jinan 250022， Shandong， China）

Abstract： To meet stationary tracking requirements of drowning rescue robots， a visual servo drowning rescue scheme based on fuzzy proportional plus derivative control was proposed. Coordinate positions of the robot were obtained through an overhead camera installed on the top of the venue， and a dynamic walking path was designed by using virtual navigation lines. Offset direction and angle were calculated， and turning rules were formulated. A triangular membership function was used to develop fuzzy proportional and derivative control rule tables. Offset angle and distance offset as well as their change rates of the robot were controlled according to proportional and derivative coefficients in the two rule tables to complete tracking and obstacle avoidance smoothness adjustment of the robot.The robot adopted kernel correlation filtering tracking algorithm to track the drowning person，and adjusted motion direction of the robot whenposition of thedrowning

收稿日期： 2022-11-24""""""""" 網絡首發時間：2023-12-25T15：04：35

基金項目： 山東省重點研發計劃項目（2017CXGC0810）；山東省教育科學“十三五”規劃教育招生考試專項課題項目（BYZK201917）

第一作者簡介： 郭英（1996—），男，山東濰坊人。碩士研究生，研究方向為模式識別與圖像處理、 機器人。E-mail： 2411159510@qq.com。

通信作者簡介： 李金屏（1968—），男，河南焦作人。教授，博士，碩士生導師，研究方向為數字圖像處理、 計算機視覺、 模式識別與人工

智能、 優化算法等。 E-mail： ise_lijp@ujn.edu.cn。

網絡首發地址： https：//link.cnki.net/urlid/37.1378.N.20231222.1717.002

person changed. The robot relied on real-time and stable sonar information to set the offset angle and achieved obstacle avoidance function. Experimental verification of the proposed scheme was conducted on Pioneer P3DX robot. The results show that the robot can respond to the change of the target in time without phenomena of mistracking and losing. All roll angles under different working conditions are less than 1 °， and the obstacle avoidance rate reaches 100%. The robot can reach the target position quickly， stably， and accurately， which meets early application requirements of drowning rescue robots.

Keywords： drowning rescue robot; stationary tracking; fuzzy proportional plus derivative control; visual servo

目前救援機器人應用在越來越多的救援場景中，如地震、 火災、 溺水等，為人類生命安全提供了重要保障。其中應用于游泳場館的溺水救援機器人涉及路徑規劃、 溺水人員的跟蹤、 避障等技術，確保救援機器人的平穩行走是救援的關鍵。

救援機器人的研制始于20世紀中期［1-2］。救援機器人在2001年美國“9·11”恐怖襲擊事件災難救援中的成功應用引發了世界各國的研究熱潮［3］。在國外研究者研發的救援機器人中，LS3型四足機器人［4］跌倒后可以自行恢復平衡，四足機器人“獵豹”［5］可以跳過高度為0.45 m的障礙物，四足機器人“野貓”［6］為當時世界上奔跑最快的救援機器人。戰場救援機器人“Bear”［7］有2種行進模式：一種是在平坦路面輪式前行；另一種是在崎嶇路面切換成履帶前行，減小顛簸，從而保護被救援人員。在國內研發的救援機器人中，六爪章魚機器人［8］承載能力強，運動靈活，路面適應力強，可在多種地形環境中執行救援任務；PH型并聯六足機器人［9］可實現對周圍環境的三維掃描，具有承載能力強、 運動靈活、 使用領域廣泛等特點。

機器人的主要避障方法分為以下幾類：基于圖論的方法、 群智能理論啟發式算法、 隨機規劃法、 基于預測的算法等。基于圖論的方法包括A*算法［10］、 維諾圖法［11］、 概率路線圖（probabilistic road maps， PRM）算法［12］等，該類方法融合貪心算法和啟發函數的思想，通過遍歷的方式尋找從起點到目標點的可行路徑，但遇到動態障礙物時需要重新規劃路徑。群智能理論啟發式算法包括粒子群算法［13］、 遺傳算法［14］、 蟻群算法［15］等，該類算法具有實現容易、 精度高、 穩健性好的優點，但需要耗費大量計算資源，難以保證復雜環境規劃的實時性。隨機規劃法包括快速隨機探索樹（rapidly-exploring random trees， RRT）［16］、RRT*［17］及雙向RRT等，該類方法構造簡單并能較快地搜索未知空間，但得到的避障路徑隨機性太強，且在避障路徑上存在曲率不連續的點，難以保證避障軌跡的平滑性。基于預測的算法在預測障礙物未來運動軌跡集合的基礎上，通過優化指標函數尋求避障路徑［18］。

面對水下救援的新場景、 新挑戰，可通過規劃虛擬導航線［19］指導機器人行走。由于水下阻力大并且跟蹤位置會發生變化，因此迄今為止尚未見到投入商用的水下救援機器人產品或應用，相關文獻也較少。在暫時不具備水下實驗條件的情況下，可進行地面實驗，利用模糊比例微分（proportion plus derivative， PD）控制機器人行走。本文中為了滿足溺水救援機器人的平穩跟蹤需求，對機器人平穩跟蹤與避障進行前期研究，提出一種基于模糊PD控制的視覺伺服方案（簡稱本文方案）。利用場館頂部安裝的攝像頭快速定位溺水人員位置，對溺水人員進行跟蹤以確認位置變化。利用核相關濾波器（kernel correlation filter， KCF）跟蹤算法對樣本的方向梯度直方圖（histogram of oriented gradient， HOG）進行跟蹤；采用聲吶進行實時檢測，并根據聲吶信息指導機器人避障；對全局攝像頭所獲取圖像的偏移距離、偏移距離變化率進行模糊處理并制定模糊PD控制規則表；利用規則表控制機器人的直行和轉彎，從而使機器人根據救援指令，通過全局攝像頭的指引能快速、 平穩、 準確地到達指定位置。

濟南大學學報（自然科學版）第38卷

第2期郭英，等：面向溺水救援機器人平穩跟蹤的模糊比例微分控制視覺伺服方案

1" 本文方案

溺水救援機器人采用的跟蹤算法是KCF跟蹤算法，利用HOG特征進行目標檢測，控制方法為模糊PD控制。通過計算誤差和誤差變化率實現對機器人的控制。

1.1" 目標檢測與跟蹤

KCF跟蹤算法的目標檢測與跟蹤流程如圖1所示。 首先， 在第1幀視頻中選擇跟蹤目標的位置和大小作為初始信息， 提取跟蹤目標的HOG特征， 訓練核相關濾波器； 其次， 在下一幀中， 以前一幀跟蹤目標位置為中心進行檢測， 計算自相關矩陣， 通過傅里葉變換和傅里葉逆變換求得候選區域的響應值，響應值最大的位置預測為當前目標位置的中心；最后，提取當前跟蹤目標的HOG特征，更新KCF。對于之后的每幀圖像，重復上述檢測與跟蹤過程。KCF跟蹤算法通過游泳場館溺水檢測系統獲取溺水人員位置，對溺水救援機器人和溺水人員進行跟蹤，以機器人與溺水人員的中點連線作為虛擬導航路徑，每隔4幀進行1次機器人調整，在機器人到達溺水人員位置后暫停并等待，當溺水人員位置改變時進行再次跟蹤。

1.2" 模糊PD控制

模糊PD控制利用模糊邏輯制定模糊規則，并對PD參數進行實時優化，以克服傳統控制參數無法實時調整的缺點。

1.2.1" 模糊PD控制流程

e（t）—t時刻的系統誤差； er—誤差變化率；

Kp—比例系數； Kd—微分系數。

圖2" 模糊比例微分（PD）控制流程［20］

模糊PD控制的數學表達式［20］為

u（t）=Kpe（t）+Kdde（t）dt ，（1）

式中： u（t）為t時刻的系統輸出； Kp為比例系數； e（t）為t時刻的系統誤差； Kd為微分系數。

離散化的模糊PD控制器為

u（k）=Kpe（k）+Kd［e（k）-e（k-1）］ ，（2）

式中： u（k）為第k次采樣時的系統輸出； e（k）為第k次采樣時的系統誤差。

令T為采樣周期，則Kd=Td/TKp，其中Td為微分時間常數。式（2）為位置式模糊PD控制。在實際應用中，常采用增量式模糊PD控制，更易于編程，即

Δuk=u（k）-u（k-1）=Kp［e（k）-e（k-1）］+

Kd［e（k）-2e（k-1）+e（k-2）］ ，（3）

式中Δuk為u（k）變化量。

令A=Kp（1+Td/T），B=KpTd/T，式（3）轉換為

Δuk=Ae（k）+Be（k-2） 。（4）

由此可知，一旦確定A、 B與前幾次的系統偏差，即可計算出控制量。

1.2.2" 偏移規則

在全局攝像頭所獲取的圖像中建立平面直角坐標系，如圖3所示。機器人當前位置與終點的連線記為L1，機器人在4幀圖像拍攝前的位置與當前位置的連線記為L2。溺水救援機器人在4幀圖像拍攝前的坐標為（x1， y1），當前坐標為（x2， y2），終點坐標為（g1， g2），可以根據這些坐標求解機器人的偏移角及偏移方向。初始方向不作限定，機器人根據偏移規則自動調整前進方向。

xoy—平面直角坐標系； θ—直線L1與L2的夾角，

其中L1為溺水救援機器人當前位置與終點的連線，

L2為機器人在4幀圖像拍攝前的位置與當前位置的連線；

（x1， y1）、 （x2， y2）、 （g1， g2）—溺水救援機器人

在4幀圖像拍攝前的坐標、 當前坐標、 終點坐標。

直線L1與L2的夾角為

θ=arctank2-k11+k1k2 ，（5）

式中k1、 k2分別為直線L1、 L2的斜率。 機器人的偏移規則如下： 當k1lt;k2且θ為負時， 機器人右轉； 當k1gt;k2且θ為正時， 機器人左轉； 當k1=k2時， 機器人不偏移。 當前進方向與目標方向呈鈍角時， 機器人首先進行180°旋轉， 然后再經過一次偏移調整即可。

1.2.3" PD控制模糊化

利用全局攝像頭獲取全局圖像，包含救援機器人與溺水人員終點目標。計算機器人的偏移距離、 偏移距離變化率、 偏移角、 偏移角變化率。對機器人的輸入進行模糊化處理，根據隸屬度函數對Kp、 Kd進行清晰化處理，得到機器人的左、 右輪速。模糊PD控制由模糊化、 模糊推理和清晰化3個部分組成，如圖4所示。

設置機器人逆時針旋轉的偏移角為負，反之為正。對偏移角區間進行模糊集劃分，定義偏移角的取值范圍為（-45°， 45°），將此區間劃分為中間，定義為零域（zero， ZO），向左分為左小（negative small， NS）、 左中（negative middle， NM）、 左大（negative big， NB）， 向右分為右小（positive small， PS）、 右中（positive middle， PM）、 右大（positive big， PB）共7個部分。救援機器人偏移角、 偏移距離模糊集劃分如表1、 2所示。

根據偏移角和偏移距離模糊集劃分，制定機器人的模糊PD控制規則表，包含直行和旋轉2種情況。機器人的調整速度為

z=v（Kp+Kd） ，（6）

式中v為機器人的前進速度。當機器人偏離程度和偏離變化率較大時，Kp、 Kd較小，機器人調整速度較慢；當機器人直行且偏離變化率較小時，Kp、 Kd較大，機器人調整速度較快。利用三角隸屬度函數［21］計算Kp、 Kd的輸出值。

1.2.4" Kp模糊控制規則

根據全局攝像頭所獲取圖像中偏移距離和偏移變化率劃分的不同區域，設計Kp調節模糊控制規則表，如表3所示。

1.2.5" Kd模糊控制規則

根據全局攝像頭所獲取圖像中偏移角和偏移角變化率劃分的不同區域，設計Kd調節模糊控制規則表，如表4所示。

由表3、 4可知，經過此模糊PD控制后，機器人行走時能夠保持平穩，保證機器人平穩地完成跟蹤與避障功能。

2" 實驗

設計實驗驗證溺水救援機器人的跟蹤、 避障性能及平穩性，從定性和定量的角度進行分析，判斷本文方案能否滿足機器人的應用要求。實驗場地在室內，長度為8 m，寬度為6 m。實驗采用美國MobileRobots公司設計的先鋒P3DX型機器人，屬于輕量級兩輪驅動型機器人。該型機器人由頂板、控制面板、聲吶環、 電機、 車輪、 電池、 蜂鳴器和網絡攝像頭等組成，如圖5所示。

3" 結果與分析

跟蹤、 避障性能及平穩性是溺水救援機器人的關鍵因素。跟蹤、 避障性能直接決定救援工作的進程和效果，也決定了機器人平穩跟蹤的準確性和可靠性。

3.1" 跟蹤性能

3.1.1" 定性分析

在實驗室場景中選取黑色箱子為目標并進行跟蹤，每隔0.4 s記錄一次機器人的前進方向。圖6所示為先鋒P3DX型機器人的跟蹤效果。由圖可知，機器人能及時應對目標變化，沒有跟錯、 跟丟現

3.1.2" 定量分析

利用平均精度（average precision， AP）計算跟蹤精度， 即跟蹤過程中跟蹤目標與機器人的平均距離。 利用歐氏距離計算機器人中心點與終點距離， 即

pavg=（f1-c1）2+（f2-c2）2 ，（7）

式中： pavg為AP； （c1， c2）為先鋒機器人所在區域的中心坐標； （f1， f2）為對應時刻跟蹤目標區域的中心坐標。

圖7所示為機器人的跟蹤精度。由圖可知：機器人在進行跟蹤的過程中，當與跟蹤目標的距離為10 cm時暫停并等待；當跟蹤目標再次移動時，機器人立刻作出反應，再次跟蹤；整個過程中AP值較小，具有較強的實用性。

3.1.3" 對比實驗

傳統跟蹤算法均值漂移（MeanShift）算法、 連續自適應的均值漂移（CAM Shift）算法、 粒子濾波算法與KCF跟蹤算法的跟蹤性能進行比較，結果如圖8所示。由圖可知，相對于傳統的跟蹤算法，KCF跟蹤算法具有精度優勢，跟蹤成功率較高，跟蹤性能較好。

3.2" 避障性能

3.2.1" 定性分析

在實驗場地布置5個障礙物進行避障實驗， 驗證避障性能。 布置障礙物的位置及機器人軌跡如圖9所示。 由圖可知， 機器人經過路徑上的5個障礙物，利用前置聲吶實時檢測運行前方障礙物，在距離障礙物12 cm處進行避障偏移，機器人平穩地躲避所有障礙物， 行走平滑， 避障準確， 達到基本應用要求。

3.2.2" 定量分析

利用機器人進行重復避障性能測試，在動態、 靜態障礙物場景中，走動的行人作為動態障礙物，固定的黑色箱子為靜態障礙物，統計避障次數與避障率，結果如表5所示。由表可知，機器人避障功能對于靜態、動態障礙物均適用，避障率為100%，多次實驗均未觸碰障礙物。

3.3" 平穩性

橫滾角表示機器人顛簸幅度，具體指機器人的水平顛簸角度，即機器人運行過程中左右擺動的幅度。機器人分別在地板磚和細小沙石路況時進行測試，并統計最大橫滾角與平均橫滾角，結果如表6所示。由表可知，機器人在地板磚及細小沙石路面的最大橫滾角與平均橫滾角均小于1°，具有良好的平穩性，達到了溺水救援機器人前期研究平穩性的要求。

4" 結論

為了滿足溺水救援機器人的平穩跟蹤需求，本文中提出一種基于模糊PD控制的視覺伺服方案，機器人在全局攝像頭的指導下進行跟蹤與避障，利用模糊PD控制保證行走的平穩性，本文方案在先鋒P3DX型機器人上進行實驗驗證，得到以下主要結論：

1）機器人能及時應對目標變化，沒有跟錯、跟丟現象，跟蹤效果完美，反應較快。當與跟蹤目標的距離為10 cm時暫停并等待；當跟蹤目標再次移動時，機器人立刻作出反應，再次跟蹤；整個過程中AP值較小，具有較強的實用性。相對于傳統跟蹤算法，KCF跟蹤算法具有精度優勢，跟蹤成功率較高，跟蹤性能較好。

2）機器人在距離障礙物12 cm處進行避障偏移，平穩地躲避所有障礙物，行走平滑，避障準確。 機器人避障功能對于靜態、 動態障礙物均適用， 避障率為100%， 多次實驗均未觸碰障礙物， 達到應用要求。

3）機器人在不同工況時的橫滾角均小于1°，具有良好的平穩性，達到了溺水救援機器人前期研究平穩性的要求。

下一步的研究方向是在溺水救援機器人到達指定位置后，利用機械臂開展溺水人員托舉工作，形成一套完整的溺水人員救援系統。

參考文獻：

［1］" NING M， MA Z F， CHEN H L， et al. Design and analysis for a multifunctional rescue robot with four-bar wheel-legged structure［J］. Advances in Mechanical Engineering， 2018， 10（2）： 1.

［2］" LI F H， HOU S K， BU C G， et al. Rescue robots for the urban earthquake environment［J］. Disaster Medicine and Public Health Preparedness， 2022， 17： e181.

［3］" 孟凱寧， 舒畇溦， 杜成群. 基于廢墟結構特征的災后救援機器人設計研究［J］. 包裝工程藝術版， 2022， 43（24）： 180.

［4］" SAPATY P S. Military robotics： latest trends and spatial grasp solutions［J］. International Journal of Advanced Research in Artificial Intelligence， 2015， 4（4）： 9.

［5］" SPRWITZ A， TULEU A， VESPIGNANI M， et al. Towards dynamic trot gait locomotion： design， control， and experiments with Cheetah-cub， a compliant quadruped robot［J］. The International Journal of Robotics Research， 2013， 32（8）： 932.

［6］" MATTUCCI F， OLIVEIRA R， BIZZARRI L， et al. Genetic structure of wildcat （Felis silvestris） populations in Italy［J］. Ecology and Evolution， 2013， 3（8）： 2443.

［7］" MILO P. Bear to the rescue［J］. Evaluation Engineering， 2011， 50（1）： 6.

［8］" 杜惠. 六足機器人容錯性能及容錯運動規劃［D］. 上海： 上海交通大學， 2016.

［9］" 宋朋. 輪腿式六足機器人機構設計與分析［D］. 秦皇島： 燕山大學， 2021.

［10］" WEINSTEIN D H. Modified Ritz method［J］. Proceedings of the National Academy of Sciences， 1934， 20（9）： 529.

［11］" BEARD R W， McLAIN T W， GOODRICH M A， et al. Coordinated target assignment and intercept for unmanned air vehicles［J］. IEEE Transactions on Robotics and Automation， 2002， 18（6）： 911.

［12］" SNCHEZ G， LATOMBE J C. On delaying collision checking in PRM planning： application to multi-robot coordination［J］. International Journal of Robotics Research， 2002， 21（1）： 5.

［13］" MO H W， XU L F.Researchofbiogeographyparticleswarm optimization for robot path planning［J］. Neurocomputing， 2015， 148： 91.

［14］" ROBERGE V， TARBOUCHI M， LABONTE G. Comparison of parallel genetic algorithm and particle swarm optimization for real-time UAV path planning［J］. IEEE Transactions on Industrial Informatics， 2013， 9（1）： 132.

［15］" 劉杰， 閆清東， 馬越， 等. 基于蟻群幾何優化算法的全局路徑規劃［J］. 東北大學學報（自然科學版）， 2015， 36（7）： 923.

［16］" 宋曉琳， 周南， 黃正瑜， 等. 改進RRT在汽車避障局部路徑規劃中的應用［J］. 湖南大學學報（自然科學版）， 2017， 44（4）： 30.

［17］" LIN D， SHEN B， LIU Y R， et al. Genetic algorithm-based compliant robot path planning： an improved Bi-RRT-based initialization method［J］. Assembly Automation， 2017， 37（3）： 261.

［18］" 彭帆， 謝永芳， 陳曉方， 等. 基于障礙物可達區域預測的機器人實時避障算法［J］. 東北大學學報（自然科學版）， 2022， 43（9）： 1225.

［19］" 梁臻， 房體育， 李金屏. 基于虛擬導航線的農業機器人精確視覺導航方法［J］. 計算機應用， 2021， 41（1）： 191.

［20］" SUGENO M. An introductory survey of fuzzy control［J］. Information Sciences， 1985， 36（1/2）： 62.

［21］" HOODA D S， MISHRA A. On trigonometric fuzzy information measures［J］. ARPN Journal of Science and Technology， 2015， 5（3）： 148.

（責任編輯：劉建亭）