雙閉環PID應用下投籃機器人路徑跟蹤控制研究

劉 鋒，夏鐵牛，李亞衛

（新疆大學體育教學研究部，新疆 烏魯木齊 830046）

1 引言

在科學技術不斷提升的情況下，機器視覺目標識別已經成為機器人跟蹤控制領域的主要技術之一［1-2］。目前視覺技術已經成為融入了工業機器人、路徑跟蹤控制技術領域，成為當前控制領域不可缺少的一部分，而視覺技術的融入也可以使企業生產更加智能化，大大的提升了企業的工作效率及工作水平。但傳統的機器人路徑跟蹤控制方法存在控制精度不佳的問題，針對我國投籃機器人路徑跟蹤控制現有缺陷，需要對投籃機器人路徑跟蹤控制方法開展深入研究。

文獻［3］提出基于改進模糊PID 的全向搬運機器人路徑跟蹤控制方法。PID全稱為全稱比例、積分、微分控制（Proportion Integration Differentiation，PID）。通過建立空間模型，將路徑跟蹤誤差作為模型輸入，而機器人電壓作為模型輸出，進一步確定控制器的取值范圍后，基于反向學習策略開展機器人優化控制處理，利用控制器實現路徑跟蹤控制。然而該方法建立的控制器不完善，存在控制效果差的問題。文獻［4］提出基于模糊PID的深海采礦機器人路徑跟蹤控制方法。首先確定了機器人當前目標位置信息，根據位置信息進一步計算機器人的目標角，通過建立機器人運動學模型對機器人的轉速差值開展有效控制，令機器人按照目標路徑行駛，為避免機器人出現打滑問題，結合分析的機器人轉輪加速度和打滑率的關系，實現機器人路徑跟蹤控制。但是該方法所采集到的數據存在較大的噪聲分類，跟蹤控制結果存在一定的誤差，存在控制精度低的問題。文獻［5］提出采用傳感器測量聯合最小二乘法的機器人路徑跟蹤控制方法。利用激光距離傳感器組建機器人測量系統，以此計算出機器人末端執行器與曲面距離，根據刀具中心點位置，獲取機器人在各個曲面中的測點位置數據。通過對近似曲面主分量的求解，進一步確定機器人未知面與拋物面參數，按照機器人期望路徑方向和刀具與曲面的任一點距離，實現機器人路徑跟蹤控制。但是該方法的計算結果不全面，存在控制誤差高的問題。

為了解決上述方法中存在的問題，提出雙閉環PID應用下投籃機器人路徑跟蹤控制方法。

2 應用OpenMv視覺識別的投籃機器人路徑識別

2.1 投籃機器人路徑圖像采集及處理

2.1.1 投籃機器人路徑圖像采集

OpenMv主要由不同型號的攝像頭模組、成像模組構成，可有效采集、處理投籃機器人路徑圖像［6］。由于C 語言包括在OpenMv硬件模塊內，聯合解釋器，在實現機器視覺的同時還可實現硬件的訪問和控制。

開展投籃機器人路徑跟蹤控制前，需要進一步確定投籃機器人總體架構，如圖1所示。

圖1 投籃機器人總體架構Fig.1 Overall Architecture of Shooting Robot

由于投籃機器人本身有多個舵機關節組，因此設定投籃機器人陀螺儀為姿態反饋。令投籃機器人的各條腿中都有不同的自由度，可完成前進、后退、轉彎等多種動作，同時將OpenMv視覺識別安裝在機器人中心位置，以此采集到更為完整的路徑圖像。

2.1.2 投籃機器人路徑圖像處理

在圖像處理單元中安裝一個OV7725攝像頭芯片，依據采集的機器人路徑圖像，在OpenMv中內置圖像處理函數，結合小波閾值算法對采集到的圖像開展去噪處理。

閾值去噪主要思想是將信號中小于某個閾值的部分（通常為噪聲）置為0或者進行某種補償處理，主要利用其去消除圖像中的噪聲，使圖像可以有更好的視覺效果。以軟閾值處理去噪方法對于圖像進行去噪處理［7-8］，通過下式表述非線性軟閾值，定義：

式中：δ—閾值水平；N—信號長度；σ—噪聲方差。

因而采用軟閾值方法對圖像的去噪流程如下：

（1）對采集的路徑圖像實施小波變換，將其應用到小波域中；

（2）對小波系數開展收縮處理；

（3）反變換小波系數，實現圖像去噪。

利用具體的閾值函數對去噪結果進行修正優化，其閾值函數定義如下：

根據設定的0 ≤α≤1取值范圍，確定軟閾值函數標準，實現圖像去噪結果的修正優化，以此獲取更高質量的圖像。

去噪前后的機器人路徑圖像對比，如圖2所示。

圖2 去噪前后的機器人路徑圖像對比Fig.2 Comparison of Robot Path Images Before and After Denoising

2.2 投籃機器人路徑識別

基于上述處理結果，將曲線擬合法與特征點檢測法結合，完成投籃機器人路徑識別。

搜索去噪后的路徑圖像，保留與閾值相符的色塊后，并從中挑選出最大色塊，將其視為目標路徑。采用曲線擬合法對機器人移動位置進行估計，從而計算出機器人偏移量，為后續投籃機器人路徑跟蹤控制奠定重要基礎。

采用曲線擬合法對機器人移動位置進行估計的流程如下：

（1）假設t、t-1、t-2 時刻投籃機器人位置分別用l(t)、l(t-1)、l(t-2)表示，則可以利用這三個點的位置確定t+1時刻投籃機器人位置，從而通過曲線擬合識別機器人移動路徑。投籃機器人位置變化示意圖，如圖3所示。

圖3 投籃機器人位置變化示意圖Fig.3 Schematic Diagram of the Position Change of the Shooting Robot

（2）對于機器人連續運動進行離散化處理，從而可以得到機器人從點l(t-1) 到l(t)之間的切向加速度bn和法向加速度bt合成結果，具體結果用以下公式表示：

式中：Δb—切向加速度bn和法向加速度bt合成值。

（3）結合Δb的計算結果，可以對投籃機器人的轉向角與分解角進行計算。其中轉向角計算結果如下：

分解角的計算公式如下：

（4）結合投籃機器人轉向角和分解角的計算結果，即可對于投籃機器人在（t+1）時刻的位置進行計算，結果如下：

（5）計算水平偏移量，結果如下：

為避免路徑出現提取錯誤的問題，結合特征點檢測算法進一步識別投籃機器人路徑。

在此之前，針對投籃機器人的路徑識別流程就如下所示：

（1）應用OpenMv視覺識別獲取路徑路段，并保存；

（2）啟動投籃機器人，利用特征點檢測算法對機器人視野中的目標路徑進行掃描；

（3）對目標路徑實施色塊框選，計算出色框坐標；

（4）利用曲線擬合法計算色框路徑偏移量；

（5）疊加路徑偏移量和色框坐標，實現投籃機器人路徑識別。

3 基于雙閉環PID控制器的機器人路徑跟蹤控制

基于曲線擬合法計算結果，令其與特征點檢測算法相結合，定位、框選出投籃機器人視野路徑元素，疊加機器人路徑偏移量后，得出實際偏移量。結合計算的路徑實際偏移量、推動器實現位置、移動速度建立雙閉環PID控制器［9-10］，利用該控制器約束投籃機器人的位姿、速度，以此跟蹤與控制期望路徑軌跡。

其中，投籃機器人主要由包括機架、萬向輪、支撐架、轉動驅動器以及控制器等組成，具體結構，如圖4所示。

圖4 投籃機器人結構Fig.4 Structure of Shooting Robot

分析圖4可知，投籃機器人利用圖像采集器采集數據的方式來識別籃球的位置，控制器會控制行走機構向籃球位置方向運動并拾起籃球，并將拾取到的籃球傳送給推動器，推動器將籃球投放到指定位置。

將推動器的機構視為封閉的矢量多邊形，則用復數形式為：

式中：L1，L2，L3，L4—矢量四邊形長度；e1，e2，e3—位移角。

結合式（8）對于推動器實現位置進行計算，結果如下：

推動器移動速度計算式如下：

式中：ω—速度角。結合上述分析，構建的雙閉環PID控制器原理圖，如圖5所示。

圖5 構建的雙閉環PID控制器原理圖Fig.5 Schematic Diagram of the Constructed Double Closed-Loop PID Controller

圖5中，投籃機器人的期望方向角由Ad表述，推進器期望速度記作Bd，實際方向角記作Aa，投籃機器人推進器實際速度由Ba表述。

根據建立的雙閉環PID控制器，將方向角誤差作為輸入，獲取控制器輸出值，結果為：

式中：KpA—投籃機器人的角度比例系數；uA(t)—當前時刻下的輸出值；KiA—積分項系數；KdA—導數項系數；eA(t)—誤差值。

結合式（11）的計算結果對投籃機器人運行期間的方向誤差校正。

將投籃機器人的驅動輪連接線速度誤差作為輸入值，標記eb，利用下式計算出輸出值：

式中：KpB—速度的比例項系數；KiB—積分項系數；KdB—微分項系數；uB(t)—輸出值。依據計算結果，將uB(t)看作投籃機器人的行駛速度控制。

由于傳統的控制方式存在調節不佳的問題，所以根據PID控制器求解出的uB(t)之后，對投籃機器人的基礎轉速控制，結合uA(t)對投籃機器人的姿態調節，實現投籃機器人路徑跟蹤控制，具體由下式表示：

式中：Tr—投籃機器人的右輪輸入值；Tl—投籃機器人左輪輸入值；K—控制后的轉換系數。

通過雙閉環PID 控制器對投籃機器人左右輪的控制，實現投籃機器人對期望軌跡的有效跟蹤，完成投籃機器人路徑跟蹤控制。

4 實驗與分析

為了驗證雙閉環PID應用下投籃機器人路徑跟蹤控制方法的整體有效性，需要對該方法開展實驗對比測試。

采用雙閉環PID應用下投籃機器人路徑跟蹤控制方法（方法1）、基于改進模糊PID的全向搬運機器人路徑跟蹤控制研究方法（方法2）和用傳感器測量聯合最小二乘法的機器人路徑跟蹤控制方法的研究（方法3）實施測試。

（1）通過采集投籃機器人路徑圖像，可知采集的圖像內存在噪聲數據，導致圖像細節不夠顯著，為有效實現投籃機器人路徑跟蹤控制，采用方法1、方法2、方法3對采集的投籃機器人路徑圖像開展去噪處理，以此驗證三種方法的處理效果。投籃機器人路徑圖像去噪對比，如圖6所示。

圖6 投籃機器人路徑圖像去噪對比Fig.6 Comparison of Path Image Denoising of Shooting Robot

分析圖6可知，采集到的圖像噪聲分量較多，通過方法1對其開展去噪處理后，圖像清晰度更加顯著，圖像質量較高；方法3去噪處理后，圖像內大部分噪聲分量去除效果明顯，但圖像內仍存有較少噪聲分量，可見方法3的去噪性能要低于方法1；方法2實施處理后，僅消除了少部分噪聲，圖像質量較差，驗證了方法2的去噪處理結果比較差。

（2）考慮到轉矩干擾對投籃機器人跟蹤控制性能產生的影響，待系統步入穩定跟蹤階段后，設置第150s時輸入階躍干擾。為此采用方法1、方法2和方法3開展加入階躍干擾后的路徑跟蹤誤差對比測試，以此驗證三種方法的路徑跟蹤控制性能，具體測試結果如下圖所示。階躍干擾下的路徑跟蹤誤差對比，如圖7所示。

圖7 階躍干擾下的路徑跟蹤誤差對比Fig.7 Comparison of Path Tracking Error under Step Interference

對比圖7（a）發現，在150s時加入階躍干擾后，方法1并未受干擾影響，跟蹤誤差持續為0，說明方法1 的橫向跟蹤控制效果好；對比圖7（b）發現，加入干擾后，三種方法的跟蹤誤差曲線均出現變化，但方法1所產生的跟蹤誤差較小，且在最快的時間內實現最佳跟蹤控制，可見方法1的跟蹤效果更好。

（3）選取投籃機器人作為研究對象，將形狀為“口”字的路徑作為跟蹤路徑，而機器人在x軸、y軸的跟蹤路徑位置表示如下：

式中：ω—路徑參數；xd(ω)—x軸的跟蹤路徑位置；yd(ω)—y軸的跟蹤路徑位置。

根據以上設置結果，采用方法1、方法2和方法3開展路徑跟蹤控制，并將控制結果與期望路徑對比，從而判斷出三種方法的跟蹤控制效果，投籃機器人路徑移動曲線，如圖8所示。

圖8 投籃機器人路徑移動曲線Fig.8 Path Moving Curve of Shooting Robot

從圖8可知，方法1的移動曲線與期望路徑一致，兩者之間并無誤差，說明該方法的投籃機器人路徑跟蹤控制效果好；方法2與期望路徑對比，在跟蹤期間出現路徑偏離的問題，驗證了方法2的投籃機器人路徑跟蹤控制效果較差；而方法3完全不按照設置的“口”字形路徑進行跟蹤，可見方法3的跟蹤效果最差。

綜上所述，方法1 的投籃機器人路徑跟蹤控制效果更好，這是因為該方法開展路徑跟蹤控制前，識別了投籃機器人路徑，為后續路徑跟蹤控制奠定了重要依據，以此增強了該方法的控制效果。

5 結束語

由于現有投籃機器人路徑跟蹤控制方法的效果差，針對這一問題，提出雙閉環PID應用下投籃機器人路徑跟蹤控制方法。利用OpenMv視覺識別系統采集了投籃機器人路徑圖像，結合小波閾值算法對采集的圖像開展去噪處理后，達到了增強圖像清晰度的目的。識別機器人路徑，利用雙閉環PID控制器實現投籃機器人路徑跟蹤控制。

通過實驗證明該方法的投籃機器人路徑跟蹤效果更高，達到了相關的研究預期。由于該方法在機器人路徑跟蹤控制方法中發揮著較大作用，日后會在該領域中有著長遠的發展前景。