閆慧君，韓玉彬

（1.河南財(cái)政金融學(xué)院，河南鄭州 450046；2.河南理工大學(xué)，河南焦作 454000）

1 引言

自人類進(jìn)入21世紀(jì)以來，智慧地球理念頻繁提出，機(jī)器人技術(shù)作為行業(yè)領(lǐng)先技術(shù)受到國家的重視[1]。在網(wǎng)球等球類的體育教學(xué)中，撿球教學(xué)環(huán)節(jié)中較為枯燥的過程，撿球機(jī)器人的出現(xiàn)成功將人工從該項(xiàng)環(huán)節(jié)中解脫出來[2]。至此，自主智能移動(dòng)機(jī)器人研究成為人們一時(shí)間熱議的話題之一。路徑規(guī)劃作為移動(dòng)式撿球機(jī)器人工作中重要的環(huán)節(jié)，對(duì)其進(jìn)行必要的視覺反饋控制成為機(jī)器人研究領(lǐng)域急需解決的問題。

文獻(xiàn)[3]提出基于偽譜法的自由漂浮柔性空間機(jī)器人姿態(tài)運(yùn)動(dòng)規(guī)劃與反饋控制。該方法采用假設(shè)模態(tài)法對(duì)機(jī)器人柔性進(jìn)行表述，利用Lagrange方法建立動(dòng)力模型，依據(jù)規(guī)劃算法完成模型求解，并將結(jié)果輸入控制系統(tǒng)中；最后根據(jù)Pontryagin原理，對(duì)參考序列的移動(dòng)軌跡跟蹤問題變換為兩點(diǎn)邊值問題，使用Legendre偽譜法對(duì)其進(jìn)行求解，實(shí)現(xiàn)對(duì)機(jī)器人避障路徑的控制。該方法由于未能在進(jìn)行反饋控制前，利用蟻群算法對(duì)移動(dòng)式撿球機(jī)器人的避障路徑進(jìn)行了規(guī)劃，所以該方法的抗干擾能力差。

文獻(xiàn)[4]提出基于自適應(yīng)RBFNN噪聲估計(jì)的自抗擾控制在姿態(tài)控制中的應(yīng)用。

該方法首根據(jù)自適應(yīng)徑向基函數(shù)實(shí)時(shí)補(bǔ)償機(jī)器人外部干擾；利用狀態(tài)誤差反饋控制方法跟蹤機(jī)械臂姿態(tài)；最后依據(jù)控制平臺(tái)實(shí)現(xiàn)機(jī)器人的反饋控制。該方法由于在補(bǔ)償函數(shù)時(shí)存在一定問題，所以該方法的控制精度低。

文獻(xiàn)[5]提出基于深度學(xué)習(xí)的單目視覺水下機(jī)器人目標(biāo)跟蹤方法研究。該方法利用深度卷積神經(jīng)網(wǎng)絡(luò)分析機(jī)器人移動(dòng)軌跡，獲取圖像深度信息估計(jì)數(shù)據(jù)；再基于圖像估計(jì)數(shù)據(jù)值對(duì)機(jī)器人運(yùn)動(dòng)目標(biāo)進(jìn)行跟蹤，實(shí)現(xiàn)控制。該方法由于在分析圖像時(shí)存在一定問題，所以該方法的抗干擾能力差。

該方法為解決上述機(jī)器人路徑視覺反饋控制方法中存在的問題，提出移動(dòng)式撿球機(jī)器人避障路徑視覺反饋控制方法。

2 路徑規(guī)劃

針對(duì)移動(dòng)式撿球機(jī)器人工作環(huán)境等相關(guān)因素，基于蟻群算法，對(duì)移動(dòng)式撿球機(jī)器人的避障路徑進(jìn)行規(guī)劃[6]。

2.1 算法描述

為規(guī)劃撿球機(jī)器人的移動(dòng)避障路徑，將撿球機(jī)器人的出發(fā)位置設(shè)定為gbegin，各個(gè)球的移動(dòng)節(jié)點(diǎn)作為食物隨機(jī)在訓(xùn)練場中分布。設(shè)定相關(guān)約束條件對(duì)撿球機(jī)器人的移動(dòng)過程進(jìn)行適當(dāng)約束。設(shè)定在訓(xùn)練場中，需撿球的位置坐標(biāo)點(diǎn)有n個(gè)，螞蟻會(huì)依據(jù)信息素對(duì)球的位置進(jìn)行判定，獲取最佳路徑，然后進(jìn)行移動(dòng)完成任務(wù)。

2.2 尋找最佳路徑

移動(dòng)式撿球機(jī)器人的撿球路徑問題實(shí)質(zhì)上是組合的極小化問題，路徑規(guī)劃過程具體流程如下：

（1）蟻群初始化

首先設(shè)定蟻群數(shù)量為m，概率的隨機(jī)數(shù)為Q0，訓(xùn)練場中的網(wǎng)絡(luò)數(shù)量為N，揮發(fā)因子用ρ表示，計(jì)算次數(shù)為T，距離啟發(fā)因子用β表示，轉(zhuǎn)角因子用γ標(biāo)記，信息素因子用α進(jìn)行表示，該算法評(píng)價(jià)函數(shù)的轉(zhuǎn)角權(quán)值為W1，距離權(quán)值為W2，訓(xùn)練場中各個(gè)球節(jié)點(diǎn)之間的距離為dij，可依據(jù)下式對(duì)其進(jìn)行計(jì)算：

式中：X、Y—球位置坐標(biāo)的兩個(gè)方向；i、j—訓(xùn)練場中第i個(gè)球的位置以及第j個(gè)球的位置。

設(shè)定訓(xùn)練場中各個(gè)球的位置為訓(xùn)練節(jié)點(diǎn)，賦予信息素τij(0)=C至場中所有節(jié)點(diǎn)路徑當(dāng)中，且τij(0)=(n，Lnn)?1，其中相鄰區(qū)域的路徑長度為Lnn，訓(xùn)練場子區(qū)域中的網(wǎng)球個(gè)數(shù)為n。撿球機(jī)器人自起始點(diǎn)移動(dòng)到最近節(jié)點(diǎn)，再由該節(jié)點(diǎn)出發(fā)移動(dòng)到剩余未到的其他節(jié)點(diǎn)中，直至回到出發(fā)位置。

（2）構(gòu)造路徑

將m只螞蟻均勻的分布在訓(xùn)練場節(jié)點(diǎn)n位置中，第k只螞蟻的移動(dòng)方向，需要依據(jù)場內(nèi)各個(gè)路徑的信息素值來確定。在第t時(shí)刻，螞蟻從i點(diǎn)移動(dòng)到j(luò)的轉(zhuǎn)移概率如下式所示：

式中:—轉(zhuǎn)移概率標(biāo)記；τ—路徑的信息量標(biāo)記；η—啟發(fā)函數(shù)標(biāo)記，機(jī)器人移動(dòng)時(shí)產(chǎn)生的轉(zhuǎn)動(dòng)角度標(biāo)記成θ形式，信息素集合標(biāo)記成s。依據(jù)上述獲取的轉(zhuǎn)移概率構(gòu)造路徑。

（3）信息素更新

信息素在進(jìn)行更新時(shí)，需要遵守局部信息素更新規(guī)則以及全局信息素更新規(guī)則[7]。螞蟻需要遍歷訓(xùn)練成內(nèi)所有球節(jié)點(diǎn)，形成搜索路徑，搜索的節(jié)點(diǎn)編碼為?，在搜索路徑上釋放信息素。在所有螞蟻都完成遍歷評(píng)價(jià)最優(yōu)路徑。過程如下式所示：

式中：Lgh—規(guī)劃后的最優(yōu)路徑。最后基于上述計(jì)算結(jié)果實(shí)現(xiàn)對(duì)移動(dòng)時(shí)撿球機(jī)器人的路徑規(guī)劃。具體流程，如圖1所示。

圖1 撿球機(jī)器人路徑規(guī)劃流程圖Fig.1 The Path Planning Flow Chart of the Ball Picking Robot

3 視覺反饋控制

基于上述規(guī)劃的撿球機(jī)器人路徑，對(duì)移動(dòng)式撿球機(jī)器人進(jìn)行避障路徑的視覺反饋控制[8]。

3.1 構(gòu)建動(dòng)力學(xué)模型

基于上述規(guī)劃的撿球機(jī)器人規(guī)劃的路徑，構(gòu)建機(jī)器人的動(dòng)力學(xué)模型。首先將移動(dòng)式機(jī)器人的左、右輪速度設(shè)定成vl和vr，旋轉(zhuǎn)角度用θ表示，平均速度設(shè)定成v，角速度標(biāo)記成ω，并通過計(jì)算獲取機(jī)器人的動(dòng)力學(xué)方程，結(jié)果如下式所示：

式中：p—撿球機(jī)器人廣義坐標(biāo)；M(p)—慣性矩陣；p—移動(dòng)狀態(tài)；T—約束力矩陣標(biāo)記成A(p)；G(p)—重力影響矩陣；λ—移動(dòng)系數(shù)；r—維的輸入矩陣標(biāo)記成τ；V(p,p)—離心率矩陣。過程中，對(duì)約束力矩陣進(jìn)行計(jì)算，結(jié)果如下式所示：

式中：η—參數(shù)矩陣；vs—移動(dòng)速度。構(gòu)建的動(dòng)力學(xué)模型如下式所示：

式中：P—構(gòu)建的動(dòng)力學(xué)模型；S(p)—零空間矩陣；vs(t)—零空間移動(dòng)速度。

計(jì)算時(shí)，需保證參數(shù)矩陣為已知數(shù)據(jù)，從而保障機(jī)器人系統(tǒng)能被有效控制。

3.2 控制策略

視覺伺服控制技術(shù)是利用多個(gè)攝像機(jī)對(duì)訓(xùn)練場地內(nèi)的數(shù)據(jù)進(jìn)行采集，從而實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)控制。

該技術(shù)基于機(jī)器人的控制系統(tǒng)中一個(gè)或多個(gè)自由度，通過對(duì)各個(gè)球位置的追蹤，實(shí)現(xiàn)控制。過程中需要對(duì)采集視覺的信號(hào)特征，實(shí)現(xiàn)機(jī)器人的期望自由度，因此需讓采集信號(hào)特征達(dá)到期望值，及時(shí)完成控制任務(wù)。

基于上述分析結(jié)果，制定撿球機(jī)器人的控制策略[9]。假設(shè)撿球機(jī)器人臂端基于左右輪位置處各裝載一個(gè)高清攝像機(jī)，對(duì)準(zhǔn)前后左右各個(gè)方位的觀測對(duì)象。采集的圖像特征向量為s，攝像機(jī)期望視角圖特征向量為s?，依據(jù)二者，可獲取圖像的誤差向量，計(jì)算結(jié)果，如下式所示：

式中：t—攝像機(jī)在的圖像誤差向量為e(t)。

由于攝像機(jī)的移動(dòng)速度與撿球機(jī)器人的移動(dòng)速度是一致的Vc=(vc，ωc)，將攝像機(jī)的瞬時(shí)線速度設(shè)定為vc，瞬時(shí)角速度為ωc，并由此計(jì)算圖像特征向量中，各個(gè)向量之間的關(guān)系，過程如下式所示：

式中：特征向量s的交互矩陣為Ls，且Le=Ls，Vc—攝像機(jī)速度。若將Vc當(dāng)做撿球機(jī)器人控制器輸入值，且誤差隨指數(shù)規(guī)律減少，結(jié)合上式獲取攝像機(jī)移動(dòng)速度與誤差之間的關(guān)系，結(jié)果如下式所示：

式中：λ—移動(dòng)系數(shù)；Le—偽逆矩陣為Le+e。

由于撿球機(jī)器人與攝像機(jī)的移動(dòng)速度一致，由上至下進(jìn)行推導(dǎo)可將獲取攝像機(jī)速度Vc看作撿球機(jī)器人的控制指令，發(fā)送至機(jī)器人控制系統(tǒng)中，保證s?s?=0從而控制撿球機(jī)器人。

3.3 控制器設(shè)計(jì)

在視覺反饋控制系統(tǒng)中，控制器的反饋信號(hào)與輸入信號(hào)獲取方式都是來自于傳感器采集的圖像數(shù)據(jù)。視覺傳感器采集的圖像數(shù)據(jù)是由三維場景投影得來。高清攝像機(jī)的成像模型，如圖2所示。

圖2 攝像機(jī)成像模型Fig.2 Camera Imaging Model

依據(jù)圖2可知，三維訓(xùn)練場坐標(biāo)為A(Xw，Yw，Zw)，a為二維坐標(biāo)中的坐標(biāo)點(diǎn)位置，坐標(biāo)值為(u，v)。首先將三維坐標(biāo)系中的坐標(biāo)位置轉(zhuǎn)化為攝像機(jī)坐標(biāo)，過程如下式所示：

式中：R—旋轉(zhuǎn)矩陣，坐標(biāo)系位置用(xc，yc，zc)表示，攝像機(jī)坐標(biāo)位置為(xw，yw，zw)，平移矩陣用τ標(biāo)記。

再將該坐標(biāo)點(diǎn)位置映射到二維平面上，獲取(x，y)，通過計(jì)算攝像機(jī)坐標(biāo)位置與二維映射坐標(biāo)位置之間的關(guān)系，獲取圖像坐標(biāo)點(diǎn)a(u，v)，結(jié)果如下式所示：

式中：f?—關(guān)系系數(shù)為；cu,cv—光心坐標(biāo)為；pupv—攝像機(jī)焦比為。

將上述獲取的圖像特征向量與雅克比矩陣相結(jié)合，實(shí)現(xiàn)對(duì)移動(dòng)式撿球機(jī)器人避障路徑的視覺反饋控制器的設(shè)計(jì)[10]。過程如下式所示：

式中：Vc—視覺反饋控制器。最后基于設(shè)計(jì)的視覺反饋控制器完成對(duì)移動(dòng)式撿球機(jī)器人避障路徑的視覺反饋控制。

4 實(shí)驗(yàn)

為了驗(yàn)證上述視覺反饋控制方法的整體有效性，需要對(duì)此方法進(jìn)行測試。

4.1 實(shí)驗(yàn)結(jié)果及分析

分別采用移動(dòng)式撿球機(jī)器人避障路徑視覺反饋控制方法（方法1）、基于偽譜法的自由漂浮柔性空間機(jī)器人姿態(tài)運(yùn)動(dòng)規(guī)劃與反饋控制（方法2）、基于自適應(yīng)RBFNN噪聲估計(jì)的自抗擾控制在姿態(tài)控制中的應(yīng)用（方法3）進(jìn)行測試；

（1）采用方法1、方法2以及方法3對(duì)撿球機(jī)器人避障路徑進(jìn)行視覺反饋控制，測試三種方法在控制過程中方向、位置誤差，測試結(jié)果，如圖3所示。

圖3 不同方法視覺反饋控制誤差檢測結(jié)果Fig.3 Different Methods of Visual Feedback Control Error Detection Results

分析圖3可知，隨著撿球機(jī)器人跟蹤距離的不斷增長，三種視覺反饋控制方法的控制誤差均呈不同程度的上升趨勢。在進(jìn)行位置誤差檢測過程中，方法3在測試初期所檢測出的誤差與方法1的檢測結(jié)果相同，但隨著跟蹤距離的增加，該方法檢測出的位置誤差呈急速上升趨勢。方法2所檢測出的誤差雖然高于方法1，但是對(duì)比方法3來看，低于方法3。綜上所述，方法1在進(jìn)行視覺反饋控制時(shí)的控制誤差小。

（2）撿球機(jī)器人在進(jìn)行工作時(shí)，會(huì)不停的在訓(xùn)練場地內(nèi)進(jìn)行活動(dòng)，采用方法1、方法2以及方法3進(jìn)行跟蹤控制時(shí)，對(duì)撿球機(jī)器人的轉(zhuǎn)向角控制量進(jìn)行測試，測試結(jié)果，如圖4所示。

圖4 視覺控制時(shí)不同方法的轉(zhuǎn)向角控制量測試結(jié)果Fig.4 Test Results of Steering Angle Control Amount of Different Methods Under Visual Control

分析圖4可知，在轉(zhuǎn)向角的檢測中，檢測出的轉(zhuǎn)向角控制量與期望轉(zhuǎn)向角控制量越相近，說明該方法控制時(shí)的控制精度越高。方法2與方法3檢測出的轉(zhuǎn)向角控制量與期望轉(zhuǎn)向角控制量之間存在較大偏差，方法1檢測出的轉(zhuǎn)向角控制量接近于期望轉(zhuǎn)向角控制量，說明方法1在進(jìn)行撿球機(jī)器人視覺控制時(shí)的控制精度優(yōu)于其他兩種方法。綜上所述，方法1在進(jìn)行撿球機(jī)器人反饋控制時(shí)的控制精度高。

（3）采用方法1、方法2 以及方法3 對(duì)角球機(jī)器人進(jìn)行反饋控制，測試三種方法在控制時(shí)的抗干擾能力，檢測結(jié)果，如圖5所示。

分析圖5可知，檢測出的干擾值越低說明控制方法的抗干擾能力越強(qiáng)。方法2在測試初期檢測出的干擾值與方法1相同，但是隨著跟蹤距離的不斷增加，該方法的干擾值呈急速上升趨勢。方法1檢測出的干擾值低于其他兩種方法，這主要是因?yàn)榉椒?在進(jìn)行反饋控制前利用蟻群算法對(duì)移動(dòng)式撿球機(jī)器人的避障路徑進(jìn)行了規(guī)劃，所以該方法在進(jìn)行跟蹤控制時(shí)的抗干擾能力強(qiáng)。

圖5 不同方法的抗干擾能力測試結(jié)果Fig.5 Anti?Interference Ability Test Results of Different Methods

5 結(jié)束語

近年來機(jī)器人制造技術(shù)飛速發(fā)展，應(yīng)用范圍廣泛。針對(duì)傳統(tǒng)機(jī)器人視覺反饋控制方法中存在的問題，提出基于移動(dòng)式撿球機(jī)器人避障路徑視覺反饋控制方法。

該方法首先利用蟻群算法對(duì)移動(dòng)式撿球機(jī)器人的避障路徑進(jìn)行了規(guī)劃；再通過構(gòu)建的動(dòng)力學(xué)模型，利用控制策略，設(shè)計(jì)視覺反饋控制器，最后通過該控制器完成對(duì)撿球機(jī)器人避障路徑視覺反饋控制。該方法由于在構(gòu)建動(dòng)力學(xué)模型時(shí)存在一定問題，今后會(huì)針對(duì)該缺陷繼續(xù)對(duì)該方法進(jìn)行優(yōu)化。