雙閉環(huán)PID應(yīng)用下投籃機(jī)器人路徑跟蹤控制研究

劉 鋒，夏鐵牛，李亞衛(wèi)

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

1 引言

在科學(xué)技術(shù)不斷提升的情況下，機(jī)器視覺(jué)目標(biāo)識(shí)別已經(jīng)成為機(jī)器人跟蹤控制領(lǐng)域的主要技術(shù)之一［1-2］。目前視覺(jué)技術(shù)已經(jīng)成為融入了工業(yè)機(jī)器人、路徑跟蹤控制技術(shù)領(lǐng)域，成為當(dāng)前控制領(lǐng)域不可缺少的一部分，而視覺(jué)技術(shù)的融入也可以使企業(yè)生產(chǎn)更加智能化，大大的提升了企業(yè)的工作效率及工作水平。但傳統(tǒng)的機(jī)器人路徑跟蹤控制方法存在控制精度不佳的問(wèn)題，針對(duì)我國(guó)投籃機(jī)器人路徑跟蹤控制現(xiàn)有缺陷，需要對(duì)投籃機(jī)器人路徑跟蹤控制方法開(kāi)展深入研究。

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

為了解決上述方法中存在的問(wèn)題，提出雙閉環(huán)PID應(yīng)用下投籃機(jī)器人路徑跟蹤控制方法。

2 應(yīng)用OpenMv視覺(jué)識(shí)別的投籃機(jī)器人路徑識(shí)別

2.1 投籃機(jī)器人路徑圖像采集及處理

2.1.1 投籃機(jī)器人路徑圖像采集

OpenMv主要由不同型號(hào)的攝像頭模組、成像模組構(gòu)成，可有效采集、處理投籃機(jī)器人路徑圖像［6］。由于C 語(yǔ)言包括在OpenMv硬件模塊內(nèi)，聯(lián)合解釋器，在實(shí)現(xiàn)機(jī)器視覺(jué)的同時(shí)還可實(shí)現(xiàn)硬件的訪問(wèn)和控制。

開(kāi)展投籃機(jī)器人路徑跟蹤控制前，需要進(jìn)一步確定投籃機(jī)器人總體架構(gòu)，如圖1所示。

圖1 投籃機(jī)器人總體架構(gòu)Fig.1 Overall Architecture of Shooting Robot

由于投籃機(jī)器人本身有多個(gè)舵機(jī)關(guān)節(jié)組，因此設(shè)定投籃機(jī)器人陀螺儀為姿態(tài)反饋。令投籃機(jī)器人的各條腿中都有不同的自由度，可完成前進(jìn)、后退、轉(zhuǎn)彎等多種動(dòng)作，同時(shí)將OpenMv視覺(jué)識(shí)別安裝在機(jī)器人中心位置，以此采集到更為完整的路徑圖像。

2.1.2 投籃機(jī)器人路徑圖像處理

在圖像處理單元中安裝一個(gè)OV7725攝像頭芯片，依據(jù)采集的機(jī)器人路徑圖像，在OpenMv中內(nèi)置圖像處理函數(shù)，結(jié)合小波閾值算法對(duì)采集到的圖像開(kāi)展去噪處理。

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

式中：δ—閾值水平；N—信號(hào)長(zhǎng)度；σ—噪聲方差。

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

（1）對(duì)采集的路徑圖像實(shí)施小波變換，將其應(yīng)用到小波域中；

（2）對(duì)小波系數(shù)開(kāi)展收縮處理；

（3）反變換小波系數(shù)，實(shí)現(xiàn)圖像去噪。

利用具體的閾值函數(shù)對(duì)去噪結(jié)果進(jìn)行修正優(yōu)化，其閾值函數(shù)定義如下：

根據(jù)設(shè)定的0 ≤α≤1取值范圍，確定軟閾值函數(shù)標(biāo)準(zhǔn)，實(shí)現(xiàn)圖像去噪結(jié)果的修正優(yōu)化，以此獲取更高質(zhì)量的圖像。

去噪前后的機(jī)器人路徑圖像對(duì)比，如圖2所示。

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

2.2 投籃機(jī)器人路徑識(shí)別

基于上述處理結(jié)果，將曲線擬合法與特征點(diǎn)檢測(cè)法結(jié)合，完成投籃機(jī)器人路徑識(shí)別。

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

采用曲線擬合法對(duì)機(jī)器人移動(dòng)位置進(jìn)行估計(jì)的流程如下：

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

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

（2）對(duì)于機(jī)器人連續(xù)運(yùn)動(dòng)進(jìn)行離散化處理，從而可以得到機(jī)器人從點(diǎn)l(t-1) 到l(t)之間的切向加速度bn和法向加速度bt合成結(jié)果，具體結(jié)果用以下公式表示：

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

（3）結(jié)合Δb的計(jì)算結(jié)果，可以對(duì)投籃機(jī)器人的轉(zhuǎn)向角與分解角進(jìn)行計(jì)算。其中轉(zhuǎn)向角計(jì)算結(jié)果如下：

分解角的計(jì)算公式如下：

（4）結(jié)合投籃機(jī)器人轉(zhuǎn)向角和分解角的計(jì)算結(jié)果，即可對(duì)于投籃機(jī)器人在（t+1）時(shí)刻的位置進(jìn)行計(jì)算，結(jié)果如下：

（5）計(jì)算水平偏移量，結(jié)果如下：

為避免路徑出現(xiàn)提取錯(cuò)誤的問(wèn)題，結(jié)合特征點(diǎn)檢測(cè)算法進(jìn)一步識(shí)別投籃機(jī)器人路徑。

在此之前，針對(duì)投籃機(jī)器人的路徑識(shí)別流程就如下所示：

（1）應(yīng)用OpenMv視覺(jué)識(shí)別獲取路徑路段，并保存；

（2）啟動(dòng)投籃機(jī)器人，利用特征點(diǎn)檢測(cè)算法對(duì)機(jī)器人視野中的目標(biāo)路徑進(jìn)行掃描；

（3）對(duì)目標(biāo)路徑實(shí)施色塊框選，計(jì)算出色框坐標(biāo)；

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

（5）疊加路徑偏移量和色框坐標(biāo)，實(shí)現(xiàn)投籃機(jī)器人路徑識(shí)別。

3 基于雙閉環(huán)PID控制器的機(jī)器人路徑跟蹤控制

基于曲線擬合法計(jì)算結(jié)果，令其與特征點(diǎn)檢測(cè)算法相結(jié)合，定位、框選出投籃機(jī)器人視野路徑元素，疊加機(jī)器人路徑偏移量后，得出實(shí)際偏移量。結(jié)合計(jì)算的路徑實(shí)際偏移量、推動(dòng)器實(shí)現(xiàn)位置、移動(dòng)速度建立雙閉環(huán)PID控制器［9-10］，利用該控制器約束投籃機(jī)器人的位姿、速度，以此跟蹤與控制期望路徑軌跡。

其中，投籃機(jī)器人主要由包括機(jī)架、萬(wàn)向輪、支撐架、轉(zhuǎn)動(dòng)驅(qū)動(dòng)器以及控制器等組成，具體結(jié)構(gòu)，如圖4所示。

圖4 投籃機(jī)器人結(jié)構(gòu)Fig.4 Structure of Shooting Robot

分析圖4可知，投籃機(jī)器人利用圖像采集器采集數(shù)據(jù)的方式來(lái)識(shí)別籃球的位置，控制器會(huì)控制行走機(jī)構(gòu)向籃球位置方向運(yùn)動(dòng)并拾起籃球，并將拾取到的籃球傳送給推動(dòng)器，推動(dòng)器將籃球投放到指定位置。

將推動(dòng)器的機(jī)構(gòu)視為封閉的矢量多邊形，則用復(fù)數(shù)形式為：

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

結(jié)合式（8）對(duì)于推動(dòng)器實(shí)現(xiàn)位置進(jìn)行計(jì)算，結(jié)果如下：

推動(dòng)器移動(dòng)速度計(jì)算式如下：

式中：ω—速度角。結(jié)合上述分析，構(gòu)建的雙閉環(huán)PID控制器原理圖，如圖5所示。

圖5 構(gòu)建的雙閉環(huán)PID控制器原理圖Fig.5 Schematic Diagram of the Constructed Double Closed-Loop PID Controller

圖5中，投籃機(jī)器人的期望方向角由Ad表述，推進(jìn)器期望速度記作Bd，實(shí)際方向角記作Aa，投籃機(jī)器人推進(jìn)器實(shí)際速度由Ba表述。

根據(jù)建立的雙閉環(huán)PID控制器，將方向角誤差作為輸入，獲取控制器輸出值，結(jié)果為：

式中：KpA—投籃機(jī)器人的角度比例系數(shù)；uA(t)—當(dāng)前時(shí)刻下的輸出值；KiA—積分項(xiàng)系數(shù)；KdA—導(dǎo)數(shù)項(xiàng)系數(shù)；eA(t)—誤差值。

結(jié)合式（11）的計(jì)算結(jié)果對(duì)投籃機(jī)器人運(yùn)行期間的方向誤差校正。

將投籃機(jī)器人的驅(qū)動(dòng)輪連接線速度誤差作為輸入值，標(biāo)記eb，利用下式計(jì)算出輸出值：

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

由于傳統(tǒng)的控制方式存在調(diào)節(jié)不佳的問(wèn)題，所以根據(jù)PID控制器求解出的uB(t)之后，對(duì)投籃機(jī)器人的基礎(chǔ)轉(zhuǎn)速控制，結(jié)合uA(t)對(duì)投籃機(jī)器人的姿態(tài)調(diào)節(jié)，實(shí)現(xiàn)投籃機(jī)器人路徑跟蹤控制，具體由下式表示：

式中：Tr—投籃機(jī)器人的右輪輸入值；Tl—投籃機(jī)器人左輪輸入值；K—控制后的轉(zhuǎn)換系數(shù)。

通過(guò)雙閉環(huán)PID 控制器對(duì)投籃機(jī)器人左右輪的控制，實(shí)現(xiàn)投籃機(jī)器人對(duì)期望軌跡的有效跟蹤，完成投籃機(jī)器人路徑跟蹤控制。

4 實(shí)驗(yàn)與分析

為了驗(yàn)證雙閉環(huán)PID應(yīng)用下投籃機(jī)器人路徑跟蹤控制方法的整體有效性，需要對(duì)該方法開(kāi)展實(shí)驗(yàn)對(duì)比測(cè)試。

采用雙閉環(huán)PID應(yīng)用下投籃機(jī)器人路徑跟蹤控制方法（方法1）、基于改進(jìn)模糊PID的全向搬運(yùn)機(jī)器人路徑跟蹤控制研究方法（方法2）和用傳感器測(cè)量聯(lián)合最小二乘法的機(jī)器人路徑跟蹤控制方法的研究（方法3）實(shí)施測(cè)試。

（1）通過(guò)采集投籃機(jī)器人路徑圖像，可知采集的圖像內(nèi)存在噪聲數(shù)據(jù)，導(dǎo)致圖像細(xì)節(jié)不夠顯著，為有效實(shí)現(xiàn)投籃機(jī)器人路徑跟蹤控制，采用方法1、方法2、方法3對(duì)采集的投籃機(jī)器人路徑圖像開(kāi)展去噪處理，以此驗(yàn)證三種方法的處理效果。投籃機(jī)器人路徑圖像去噪對(duì)比，如圖6所示。

圖6 投籃機(jī)器人路徑圖像去噪對(duì)比Fig.6 Comparison of Path Image Denoising of Shooting Robot

分析圖6可知，采集到的圖像噪聲分量較多，通過(guò)方法1對(duì)其開(kāi)展去噪處理后，圖像清晰度更加顯著，圖像質(zhì)量較高；方法3去噪處理后，圖像內(nèi)大部分噪聲分量去除效果明顯，但圖像內(nèi)仍存有較少噪聲分量，可見(jiàn)方法3的去噪性能要低于方法1；方法2實(shí)施處理后，僅消除了少部分噪聲，圖像質(zhì)量較差，驗(yàn)證了方法2的去噪處理結(jié)果比較差。

（2）考慮到轉(zhuǎn)矩干擾對(duì)投籃機(jī)器人跟蹤控制性能產(chǎn)生的影響，待系統(tǒng)步入穩(wěn)定跟蹤階段后，設(shè)置第150s時(shí)輸入階躍干擾。為此采用方法1、方法2和方法3開(kāi)展加入階躍干擾后的路徑跟蹤誤差對(duì)比測(cè)試，以此驗(yàn)證三種方法的路徑跟蹤控制性能，具體測(cè)試結(jié)果如下圖所示。階躍干擾下的路徑跟蹤誤差對(duì)比，如圖7所示。

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

對(duì)比圖7（a）發(fā)現(xiàn)，在150s時(shí)加入階躍干擾后，方法1并未受干擾影響，跟蹤誤差持續(xù)為0，說(shuō)明方法1 的橫向跟蹤控制效果好；對(duì)比圖7（b）發(fā)現(xiàn)，加入干擾后，三種方法的跟蹤誤差曲線均出現(xiàn)變化，但方法1所產(chǎn)生的跟蹤誤差較小，且在最快的時(shí)間內(nèi)實(shí)現(xiàn)最佳跟蹤控制，可見(jiàn)方法1的跟蹤效果更好。

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

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

根據(jù)以上設(shè)置結(jié)果，采用方法1、方法2和方法3開(kāi)展路徑跟蹤控制，并將控制結(jié)果與期望路徑對(duì)比，從而判斷出三種方法的跟蹤控制效果，投籃機(jī)器人路徑移動(dòng)曲線，如圖8所示。

圖8 投籃機(jī)器人路徑移動(dòng)曲線Fig.8 Path Moving Curve of Shooting Robot

從圖8可知，方法1的移動(dòng)曲線與期望路徑一致，兩者之間并無(wú)誤差，說(shuō)明該方法的投籃機(jī)器人路徑跟蹤控制效果好；方法2與期望路徑對(duì)比，在跟蹤期間出現(xiàn)路徑偏離的問(wèn)題，驗(yàn)證了方法2的投籃機(jī)器人路徑跟蹤控制效果較差；而方法3完全不按照設(shè)置的“口”字形路徑進(jìn)行跟蹤，可見(jiàn)方法3的跟蹤效果最差。

綜上所述，方法1 的投籃機(jī)器人路徑跟蹤控制效果更好，這是因?yàn)樵摲椒ㄩ_(kāi)展路徑跟蹤控制前，識(shí)別了投籃機(jī)器人路徑，為后續(xù)路徑跟蹤控制奠定了重要依據(jù)，以此增強(qiáng)了該方法的控制效果。

5 結(jié)束語(yǔ)

由于現(xiàn)有投籃機(jī)器人路徑跟蹤控制方法的效果差，針對(duì)這一問(wèn)題，提出雙閉環(huán)PID應(yīng)用下投籃機(jī)器人路徑跟蹤控制方法。利用OpenMv視覺(jué)識(shí)別系統(tǒng)采集了投籃機(jī)器人路徑圖像，結(jié)合小波閾值算法對(duì)采集的圖像開(kāi)展去噪處理后，達(dá)到了增強(qiáng)圖像清晰度的目的。識(shí)別機(jī)器人路徑，利用雙閉環(huán)PID控制器實(shí)現(xiàn)投籃機(jī)器人路徑跟蹤控制。

通過(guò)實(shí)驗(yàn)證明該方法的投籃機(jī)器人路徑跟蹤效果更高，達(dá)到了相關(guān)的研究預(yù)期。由于該方法在機(jī)器人路徑跟蹤控制方法中發(fā)揮著較大作用，日后會(huì)在該領(lǐng)域中有著長(zhǎng)遠(yuǎn)的發(fā)展前景。