許洋洋，李 偉，王 杰

1.鄭州工業(yè)應用技術學院 機電工程學院，鄭州451100

2.鄭州大學 電氣工程學院，鄭州450001

基于視覺的控制一直是計算機視覺和控制領域的一個研究課題，其最主要的應用就是在機器人領域[1-3]。由于視覺數據的復雜性，圖像處理算法所花費的時間不僅很長，而且隨著圖像的顏色、特征的可檢測性和擴展性以及分割的可能性等特性的不同，所花費的時間也會有很大的變化[4]。此外，視覺算法本身會做出復雜的決策，這也會帶來顯著的運行延時[5-7]。因此為了減少延時，現有的方法種類很多，如，Horsssen等人[8]通過在驅動策略中增加閾值，來減小延時，Chang等人[9]通過結合控制和低延時通信約束來解決該延時問題。但這些方法都存在運算復雜、實際部署困難等缺點。另外，在圖像處理中由于會存在大量異常數據，所以隨機樣本一致性（RANSAC）[10]被廣泛使用于消除數據的異常值，如王可等人[11]提出在RANSAC框架下建立全概率混合模型來處理圖像，李秀智等人[12]利用RANSAC結合最小二乘法來提高算法精度。但由于這類方法的特性，需要花費大量時間來運行，導致產生延時。為了解決延時問題，在此基于Prakash等人[13]提出的方法，提出了一個基于期限和事件驅動控制算法（DEC），解決視覺機器人在運行中的延時問題，并同時考慮了RANSAC方法運行過程中的延時問題。DEC方法的核心思想是以固定的周期更新閉環(huán)系統，但前提是延時小于限時，并且事件驅動的控制算法在事件發(fā)生時立即使用新數據。為了驗證提出方法的性能，在此使用了一個全向機器人通過對攝像機獲得圖像的處理，讓其在有彩色標記的環(huán)境中移動。

1 系統描述和問題表述

如圖1所示，一個全向機器人在帶有彩色標記的平面場上移動。機器人配備了一個車載攝像頭，可以觀察機器人前方的環(huán)境。通過Wi-Fi連接，基站（包含外部處理和控制平臺）獲取攝像機圖像并對其進行處理，從而對機器人在環(huán)境中的位置估計。利用位置估計，控制器確定速度反饋控制信號，該信號通過Wi-Fi連接傳輸回機器人。假設Wi-Fi連接中的延時與圖像處理延時相比可以忽略不計。機器人計算并應用對應于反饋信號的適當車輪運動，從而達到流程的循環(huán)。

圖1 全向機器人的基于圖片的反饋流程Fig.1 Feedback processing of omnidirectional robots based on picture

將機器人建模并控制為二維地面上的剛體。如圖2所示，在此引入一個機器人自身位置框架{R}與世界位置框架{W}之間關系。機器人框架的原點固定在機器人三角形底座的幾何中心。對于這兩個幀，前兩個坐標軸位于曲面上，最后一個坐標軸指向曲面外，因此和是平行的。機器人有三個自由度：沿和方向平移x和y，繞旋轉ψ。

圖2 標記的環(huán)境下機器人俯視圖Fig.2 Top view of robot in marked environment

p=[x,y,ψ]T表示機器人相對于世界坐標系的姿態(tài)。機器人的速度用表示?；拘D矩陣如公式（1）：

基本旋轉矩陣將機器人框架中的一個姿態(tài)與世界框架中的一個姿態(tài)聯系起來，這樣就可以寫出世界框架中的速度，對于t∈?≥0，可用公式（2）計算知：

將機器人基于圖像的反饋控制輸入uC作為機器人的速度設定值，即vR(t)=uC(t),t∈?≥0。本文主要研究x和y方向上的運動，為了應用文獻[1]中的線性系統設計，假設機器人的方向是固定的，即對于所有t∈?≥0，假設ψ(t)=ψ0，其中ψ0∈[ 0 ,2π)。從公式（2）可以看出，系統動力學是由線性模型給出的，如公式（3）所示：

如圖2所示，相機位于地平面上的機器人框架中的位置pO=[xO,yO,ψO]T處地面上的固定高度h∈?≥0處，并且在表面上具有觀察框架{O}。定位算法用于根據攝像機圖像計算機器人位置的估計值，表示為。

2 控制問題

機器人的目標是遵循預定的參考軌跡pref:?≥0→?2×[0,2)其中與機器人旋轉相對應的第三個元素是所有t∈?≥0的等于ψ0的常數。公式（3）對應的前饋控制動作uFF在機器人中實現，詳見第3章。由于模型失配和干擾，真實系統與公式（3）不完全匹配，機器人偏離期望軌跡pref。

對模型失配的影響和擾動進行建模，作為p元素上獨立的連續(xù)時間加維納過程（Wiener processes），在離散時間內，p元素可以看作是加性高斯白噪聲擾動。因此，可以考慮標準形式的連續(xù)時間的隨機微分方程載體模型，如公式（4）所示：

在這里Ac=0,Bc=Rz(ψ0)，并且Bw=diag([Bw,x,Bw,y,Bw,ψ])，式中Bw,x、Bw,y、Bw,ψ為擾動的標準差，p(t)∈為狀態(tài)，u(t)∈為t∈?≥0時的應用控制輸入，w為nw維納過程，在Inwdt中協方差遞增，其中Inw為nw×nw大小的恒等矩陣。注意，由于Inw和Bw不是時變的，所以噪聲的方差在連續(xù)時間內不是時變的。注意，雖然dw是標準白噪聲，但Bwdw的單位在物理上對應于線性和角度位置的位移擾動。此外，u=uFF+uFB，其中uFB是控制反饋項。注意(Ac,Bc)是可控的，Bc是滿秩可逆的。

控制目標是線性二次高斯型成本函數的最小化，如公式（5）所示：

這里，gc(e,u)=eTQce+uTRcu，具有正定矩陣Qc,Rc?0，其中包括偏離參考軌跡pref(t)的成本和應用反饋控制動作uFB(t)的懲罰，其中：

在公式（6）中，在這里uk∈?3和tk控制更新時間k∈?，最小化式（4）受隨機定位延時和過程擾動的影響。在本文中，證明了可以用概率分布函數（probability distribution function，pdf）f:?>0→? 或相應的累積分布函數（cumulative distribution function，cdf）F:?>0→[0,1]描述的隨機過程對拍攝圖像和將姿態(tài)估計值p呈現給控制器的時間之間的延時τ來建模。為了解決具有隨機測量時滯的控制問題，將應用文獻[13]中提出的隨機驅動時滯系統的控制設計思想。第3章詳細介紹了控制策略，以及如何在當前情況下應用這些策略。

3 控制設計

在本章中，將解釋在機器人中實現的控制算法。速度控制輸入u()

t,t∈?是數字實現的，具體如公式（7）：

首先，在此討論了前饋控制uFF，它用于引導機器人沿著參考軌跡運動。其次，討論了基于圖像的反饋控制uFB，它可以校正與參考值的偏差，并且最小化公式（5）。完整的參考跟蹤控制方案如圖3所示。

圖3 基于前饋和視覺反饋的控制方案Fig.3 Control scheme based on feedforward and visual feedback

3.1 前饋控制

在此由于前饋更新率明顯高于反饋更新率，因此可以認為前饋對于反饋問題是理想的。對于給定的可微參考軌跡pref:?≥0→?2×[ 0 ,2π)，考慮公式（4），當Bw=0時，所有t∈?滿足p(t)=pref(t)的輸入uFF：

由于Bc=Rz(ψ0)是一個旋轉矩陣，它總是可逆的。近似于公式（8），在數字域中，在的時間間隔內，對應于20 Hz的更新率，在此實現了t∈[nTFF,(n+1)TFF),n∈? ，分段常數輸入，如公式（9）：

在這里，如果Bw=0，則p(nTFF)=pref(nTFF),n∈?。

3.2 隨機時滯反饋控制

為了補償前饋控制不可能實現的干擾w的影響，在跟蹤控制配置中使用攝像機圖像。如前文所述，局部化步驟可以用pdff的隨機延時來建模。用e表示在時間tk獲得的圖像在所有k∈?處的處理延時，對于在此的設置來說，這是分布f的獨立同分布（independently identically distribution，iid）實現。接下來，將研究對于公式（6）和基于pdff和cdfF的測量延時，在這里可以將文獻[9]的思想用于控制設計。

首先，在這里定義了跟蹤誤差e(t)=p(t)-pref(t),t∈?≥0，根據公式（4）和（8）得到了誤差動力學模型，如公式（10）：

通過對誤差系統（10）在采樣時間tk，k∈?的離散化，那么可以得到公式（11）：

在這里，hk可以通過tk+1-tk計算得到，A(h)為eAch，擾動是一個具有協方差的零均值獨立隨機向量wk∈?nw,k∈?序列，其中W(h)=代表h∈?≥0。注意，自協方差W(hk)僅取決于采樣間隔hk，而不取決于采樣實例tk。

平均成本（5）可以改寫成公式（12）：

式中，N()

T為并且g( )

e,u,h=，這是與維納過程的內部采樣行為相關聯的成本。

由于提出的系統具有簡單的動力學，Ac=0,Bc=Rz(ψ0)，那么得到和接下來，將解釋此系統的期限驅動和事件驅動控制方案。

3.2.1 期限驅動的控制

在期限驅動的控制中，根據固定的期限，例如hk=D∈?≥0對于所有的k∈?，控制會更新，如公式（13）：

是當前狀態(tài)ek的估計值，使用卡爾曼濾波器獲得公式（14）：

在公式（15）中，常數D∈?>0可以任意選擇。注意，τk-1≤D的概率由F(D)給出。在這假定獲得完整狀態(tài)的完美測量，這是所選圖像采集和圖像處理方法的結果。估計增益由Ld=A()D給出，估計eˉk的無窮時域協方差Θd由李雅普諾夫（Lyapunov）方程的解給出，如公式（16）：

它只對D∈?的值有一個解，其中A(D)是舒爾數（Schur）。因為在這里的例子中A(D)=I且D∈?>0，如果F(D)>0，則是舒爾數。對于D∈?≥0的特定值，此控制設計最小化公式（12），最小值由公式（17）可知：

通過計算公式（17）的大量D∈?的值，確定了使Jd最小化的D的最優(yōu)值。

由于hk=D∈?>0是截止日期驅動情況下所有k∈?的常數，因此這種控制類型的控制更新是周期性的。

3.2.2 事件驅動控制

在事件驅動控制中，該控制在處理后立即更新，即對所有k∈? 的hk=τk，可獲得公式（18）：

式中，τ具有可由勒貝格-斯蒂爾杰斯（Lebesgue-Stieltjes）積分計算。注意，在這種情況下，總是獲得姿勢估計，但是延時τk和因此反饋控制更新間隔hk是概率性的。注意，雖然期限驅動的方法的協方差W(hk)是恒定的，因為hk=D，但由于采樣間隔依賴于變化的延時τk，因此對于事件驅動的方法，它是時變的。估計增益由給出，估計值eˉk的無窮時域協方差由給出。

對于事件驅動的情況，這種控制設計最小化公式（12），最小值由公式（21）得出：

在這里，是延時的預期值。

4 基于視覺的定位

同時定位和映射（SLAM），是機器人研究中需要解決的問題。為了解決這個問題，通常使用SIFT和SURF等特征檢測器。然而，在本文中，不需要像SLAM中那樣對環(huán)境進行映射，并且特征具有簡單的幾何圖形。因此，只考慮定位問題，不需要SIFT和SURF等高級特征檢測器。RANSAC[10]方法經常被用來匹配一系列圖像中的特征。由于主要關注隨機延時控制算法的性能，在此考慮了一個簡單的基于視覺的定位過程。

攝像機相對于{R}的精確三維姿態(tài)及其固有參數，包括幾何畸變模型[14]，是通過使用來自MATLAB的計算機視覺工具箱攝像機校準器通過攝像機校準先驗確定的。通過從相機姿態(tài)的精確透視變換，利用所有標記都位于同一平面的事實，可以對固定{R}的觀測幀{O}進行計算。基站中的圖像處理軟件有一個數據庫，其中包含環(huán)境中的所有標記及其顏色和相對于世界幀{W}的位置。基站接收到新的攝像機圖像后，通過使用失真校正、色調飽和值（HSV）顏色分割[15-16]、形態(tài)清理和閉合、blob分析[17-18]等方法，精確地確定彩色標記相對于{O}的中心的顏色和位置。然后，它實現了一個基于RANSAC的定位算法，將觀察到的標記的配置與數據庫中隨機標記集的配置相匹配。找到一個正匹配的結果，估計機器人在環(huán)境中的姿態(tài)。由于算法的隨機性和無記憶性，算法的完成時間是隨機的、獨立的、同分布的。

這里，i∈{ }1,2,…。如圖2所示，標記的顏色及其相互之間的歐幾里德距離，可以由公式（23）計算得到：

同樣的，對于i∈{1 ,2,…}。如圖2所示，以及顏色和歐氏距離l(oi,oj),i,j∈{1 ,2,…}。注意，e1O、eO2是圖2中所示的幀{O}的坐標軸。

機器人的姿態(tài)可以通過匹配O和D中的元素來確定。將公式（24）中定義ox和oy，o∈O的一個元素，與公式（22）中定義的dx,dy d∈D的一個元素的正確匹配，其需要滿足以下關系，如公式（25）：

其中，pO=[xO,yO,ψO]T是坐標變換后的框架{O}關于框架{R}的姿態(tài)。注意，pO=[xO,yO,ψO]T是固定且已知的，并且公式（25）在旋轉ψ中是非線性的。

注意公式（25）只提供了三個未知量( )x,y,ψ的兩個非線性方程，這不足以獲得唯一的姿態(tài)。因此，需要同時從每個集合O和D中得到兩個或多個標記，這就提供了三個未知量的四個或多個方程。在此分兩步解決這個問題。首先，求解非線性差分方程（有兩個標記o1,o2∈O,d1,d2∈D的情況），如公式（26）所示：

對于初始值為ψ0的牛頓方法[19]中的。其次，用公式（25）中的ψ代替每個標記中的，并用最小二乘法求解x和y，得到估計和通過考慮更多的標記，可以使姿態(tài)估計對測量誤差更具魯棒性。

針對O和D中選擇標記用于解決位姿估計問題的開放性問題，在此基于RANSAC方法來解決這類問題。具體的應用算法如算法1所示。

算法1RANSAC位置估計

步驟1隨機采樣

隨機選擇oi,oj∈O,i≠janddl,dm∈D,l≠m搭配成對的顏色(i,j)和(j,m)。對于一些小容忍度?1，檢測環(huán)境

條件（15）檢查成對(i,j)和(l,m)的歐氏距離是否相同，否則成對不能對應于匹配。如果滿足公式（15），則繼續(xù)執(zhí)行步驟2，否則返回步驟1并獲取新的標記對。

步驟2模型估計

步驟3達成共識

隨機選擇一個元素ok∈O{ }oi,oj。將ok映射到世界坐標系中的點

對于O{oi,oj}中的所有其他元素，可以重復此過程。如果達到所選擇的足夠數量的解釋標記（至少一個），則算法接受姿勢p等于估計否則，算法返回到步驟1。

5 實驗與評估

在這一部分中，解釋了識別相關系統參數所采取的步驟，并提供了實驗結果。

5.1 實驗設置

在這里采用SuperDroid Robots的全向機器人作為實驗平臺，該機器人有三個全向車輪，由IG32 265轉/分齒輪電機驅動，帶有正交編碼器（每轉538次計數）和齒輪箱（傳動比為0.052 1），分別由Arduino UNO以1 kHz的頻率控制。在樹莓派Pi 3上實現，通過網絡攝像頭（型號為羅技C525）獲取圖像，與基站的通信以及向Arduino UNO發(fā)送指令的功能。使用專用無線路由器（型號為D-link DIR-850L）通過用戶數據報協議連接傳輸無線數據。處理和控制平臺（基站）為英特爾i7 2.40 GHz處理器，8 GB內存，系統為Windows 7 64位。

在實驗過程中，外部計算機以fvalidate=25 Hz的快速速率對攝像機進行采樣，以獲得驗證數據。

從相機獲得320×240 RGB像素的圖像。作為姿勢估計參數，?。?1=0.1，對ψ的公差為10-6，至少需要三個標記，并且至少85%的觀察標記應該用?2=0.015 m的數據庫來解釋。

如圖4所示，由于環(huán)境中重復了多個標記模式，因此姿態(tài)可能由于這些偽影而被錯誤地估計。由于本文不關注這種情況，所以添加了一個額外的檢查，檢查接受的姿態(tài)是否在當前姿態(tài)估計值的0.08 m范圍內。如果不是這樣，繼續(xù)定位算法。正如接下來將看到的，過程噪聲足夠小，因此很難觸發(fā)此閾值。

圖4 實驗環(huán)境示意圖Fig.4 Experimental environment

雖然可以使用更精細的方案來改變圖像大小，壓縮方法可以限制對延時變化的影響，但是本文假設成像和處理方法具有固定的設置，并且不考慮變化。

5.2 環(huán)境和參考軌跡

機器人的工作空間包含65個彩色標記（25個紅色、18個綠色、11個黃色和11個藍色），這些標記隨機放置在距離10 cm的正方形網格上。

機器人原點{R}選擇在機器人的中心，如圖2所示。在實驗過程中，使用參考橢球軌跡，如公式（27）所示：

表1中列出的參數，如圖4中機器人工作區(qū)所示。在每次實驗之前，機器人被驅動到精度很高，因此可以假設p(0)=pref(0)。

表1 參考橢球的參數Table 1 Parameters of reference ellipsoid

對基于高階視覺的控制系統的性能感興趣，通過使用非零速度的平滑參考軌跡來消除無法直接建模/控制的起止摩擦效應，從而減輕了機器人低階控制中的非線性效應。選擇橢球體作為圓形路徑上的變體，以創(chuàng)建更為多樣化的場景。

5.3 圖像處理模型

實驗確定了算法1中各步驟處理圖像數據所需時間的概率分布函數。通過開環(huán)實驗，僅利用前饋控制動作，即可獲得具有代表性的一系列圖像。在控制和處理平臺上用算法1對圖像序列進行處理，可以得到一組大的延時值。計算延時值的標準化直方圖提供了一個離散的概率分布。對于提出的系統，延時是以0.001 s間隔的分辨率確定的。從直方圖中，可以得到一個分段常數cdfF，它可以用來計算前面的控制器。

5.4 噪聲參數識別

在fvalidate=25 Hz的外部計算機上獲得的驗證數據，用于計算機器人在間隔下的200 s前向實驗軌跡。在使用間隔為ΔT∈?>0的線性插值來獲得機器人實際連續(xù)軌跡的近似值。

利用前饋輸入和實際位置可以確定過程噪聲參數Bw。既然uFF是已知的，可以通過公式（28）：

確定過程噪聲在區(qū)間[ti,ti+ΔT]內的總影響w(ti,ΔT)，其中ti∈?[0,200]。這里，假設uFF在這個區(qū)間內近似為常數[ti,ti+ΔT]，這是由參考軌跡的平滑度支持的。由于隨機性，w(ti,ΔT)的值隨插值時刻ti的變化而變化。使用公式（28）計算過程噪聲，ΔT=0.08 s（即，兩倍于奈奎斯特采樣（Nyquist sampling）間隔Tvalidate）。計算了噪聲的自相關，表明噪聲實例之間幾乎沒有相關性。

圖5中的直方圖描繪了處理噪聲的值在狀態(tài)上的概率分布。結果表明，該分布近似為高斯分布，平均和標準差為零，[σx(ΔT),σy(ΔT),σψ(ΔT)]=[0.003 7,0.003 7,0.007 1]。顯然，前面內容中討論的偽影閾值遠高于標準偏差。正態(tài)性檢驗表明，對于wx，可能存在高斯性的輕微偏差。因此假設所有干擾都是高斯的。

圖5 實驗過程噪聲概率分布(ΔT=0.08)Fig.5 Noise probability distribution during experiment(ΔT=0.08)

5.5 控制目標和成本

在此認為成本公式（5）由Qc=qcI和Rc=rcI定義，qc=100和rc=50。利用這些值和先前獲得的延時和過程噪聲參數，計算了理論性能通過公式（17）和（21）。通過分析，可以得到了圖6所示期限的折衷值。最佳期限為Dd=0.347 s并且Jd=0.132。當Je=0.126和τˉ=0.334 s時，事件驅動控制器的性能預計將提高4.78%。

圖6 期限D的不同值的理論平均成本Fig.6 Theoretical average cost of different values of deadline D

使用前文中的方法，可以發(fā)現Ld=Le=I，Kd=

將結果與最壞情況下期限驅動設計（使用期限Dd-con=1 s）進行比較，該設計對應于傳統的周期設計，其中99%的圖像在期限內產生姿態(tài)估計。增益有

對于Jd-con=0.219的預期性能，這比建議的設計差65%~75%。相反，所提出的設計預計比傳統設計的性能提高約40%。

5.6 控制性能測試

在這一節(jié)中，通過對t∈[0 ,100]（單位：s）的每種控制器進行實驗來驗證理論結果，其中機器人遵循參考橢球5次。為簡潔起見，在這里分別利用e、d和d-con表示事件驅動、最優(yōu)期限驅動和最壞期限控制策略。圖7和8分別描述了每個策略的姿態(tài)和姿態(tài)誤差。

圖7 不同控制下的機器人姿態(tài)Fig.7 Robot posture under different controls

圖8 不同控制下的姿態(tài)誤差Fig.8 Posture error under different control

反饋輸入如圖9所示。保守控制器的誤差通常比預期的大，從而導致較大的控制輸入。根據前文可知，Bw=diag([ 0.013,0.013,0.025])。因此，對于Dd=0.347 s和Dd-con=1 s的期限驅動方法，自協方差分別由W(Dd)=diag[( 5.86×10-5,5.86×10-5,21.7×10-5)]和W(Dd-con)=diag[( 16.9×10-5,16.9×10-5,62.5×10-5)]給出。對于事件驅動的方法，自協方差依賴于變化的延時。三種方法的自協方差如圖10所示。

圖9 不同控制下的反饋輸入Fig.9 Feedback input under different controls

圖10 離散時間的干擾協方差Fig.10 Interference covariance in discrete time

為了只分析平均成本（5），省略了瞬態(tài)啟動行為（最多7 s）。利用誤差和輸入數據，計算了=0.057 1，=0.056 4和=0.222 8方法的實驗性能。這表明事件驅動控制器的性能確實略好于理論結果所預期的最優(yōu)期限驅動控制器（1.3%），并且兩種方法的性能都顯著優(yōu)于最壞設計（幾乎75%）。實驗結果支持理論結果，特別是所提出的處理隨機延時的方法優(yōu)于傳統的最壞情況周期設計。當接近理論成本時，和的實驗性能明顯低于Jd和Je的理論成本。這是由于未建模效應導致很小的干擾被很好地抑制。

在圖11中，描繪了每種方法的處理延時，顯示出了在期限驅動的實現的期限（0.33 s和1 s）處延時的累積。計算樣本的自相關，顯示樣本之間沒有顯著相關性，從而驗證了iid假設。

圖11 處理每種控制方法的延時直方圖Fig.11 Delay histogram of each control method

反饋采樣周期的選擇由圖像處理步驟決定，由于隨機干擾的攻擊性有限，在20 Hz時的前饋速率的選擇足以近似地跟隨所選擇的參考。

6 總結和展望

在本文中，通過實驗證明了基于模型的事件驅動和基于期限驅動控制算法在實時視覺反饋控制中的優(yōu)勢。結果表明，基于模型的隨機測量延時方法可以應用到實際中，與傳統的（周期最壞情況）設計相比，具有顯著的性能提高?？紤]到具有固定方向的全向機器人，可以應用線性系統控制框架來控制二維位置；考慮具有非完整約束的機器人，需要擴展到非線性領域，需要進一步研究控制框架。在未來的工作中，更為復雜的機器人模型、更為智能的定位方法模型和魯棒性研究將成為研究的熱點。