張德偉，劉海濤，王仙業(yè)，肖聚亮

（天津大學(xué)機(jī)構(gòu)理論與裝備設(shè)計(jì)教育部重點(diǎn)實(shí)驗(yàn)室，天津300354）

移動(dòng)機(jī)器人具有更大的操作空間和更高的靈活性，被廣泛的應(yīng)用在各種場(chǎng)合。但由于Brockett 條件[1]的限制，移動(dòng)機(jī)器人通常需要借助額外的狀態(tài)信息來(lái)解決因自身非完整約束而導(dǎo)致的控制系統(tǒng)漸進(jìn)穩(wěn)定性和反饋穩(wěn)定性問(wèn)題。利用視覺(jué)傳感器的視覺(jué)伺服控制技術(shù)具備獲取信息全面、可靠性高等優(yōu)點(diǎn)，能夠有效提高移動(dòng)機(jī)器人在非結(jié)構(gòu)環(huán)境中的作業(yè)靈活度和智能化水平，并提供額外的狀態(tài)信息構(gòu)建移動(dòng)機(jī)器人的反饋控制系統(tǒng)，因此近年來(lái)得到了快速的發(fā)展[2-5]。

視覺(jué)伺服控制系統(tǒng)根據(jù)視覺(jué)反饋信息的類(lèi)型大致可分為：基于位置的視覺(jué)伺服PBVS（Positionbased visual servoing）和基于圖像的視覺(jué)伺服IBVS（Image-based visual servoing）[6]。PBVS控制方法需要在三維空間中設(shè)計(jì)控制器，并根據(jù)目標(biāo)對(duì)象的圖像特征估計(jì)相機(jī)自身的三維空間姿態(tài)。而IBVS方法直接基于目標(biāo)的圖像特征設(shè)計(jì)控制器，無(wú)需三維重建的過(guò)程。此外，亦可將兩種方法進(jìn)行組合，從而構(gòu)建混合視覺(jué)伺服方案[7]，利用視覺(jué)傳感器捕獲的圖像信息構(gòu)造反映機(jī)器人實(shí)際位姿的狀態(tài)變量，作為反饋信息，實(shí)現(xiàn)機(jī)器人的視覺(jué)伺服控制?；谏鲜?種方法，眾多研究者對(duì)輪式機(jī)器人的視覺(jué)伺服控制開(kāi)展了大量的研究工作。Shi等[8]采用模糊算法提高了圖像特征檢測(cè)的精度，并通過(guò)使用兩個(gè)獨(dú)立的伺服增益解耦機(jī)器人運(yùn)動(dòng)的平移和旋轉(zhuǎn)，提高了IBVS控制系統(tǒng)的性能?；贗BVS方法，Ke等[9]設(shè)計(jì)了一種管道型模型預(yù)測(cè)控制器使機(jī)器人可以更準(zhǔn)確地到達(dá)指定位置。MacKunis等[10]根據(jù)參考圖像中目標(biāo)點(diǎn)與實(shí)時(shí)圖像中對(duì)應(yīng)點(diǎn)的投影幾何關(guān)系提出一種混合視覺(jué)伺服控制方法，實(shí)現(xiàn)了輪式移動(dòng)機(jī)器人的跟蹤控制。Zhang 等[11]提出了一種基于投影單應(yīng)性技術(shù)的方法用于估計(jì)相機(jī)的內(nèi)部參數(shù)，并設(shè)計(jì)自適應(yīng)控制器解決了輪式移動(dòng)機(jī)器人在輸入飽和情況下的軌跡跟蹤和調(diào)節(jié)問(wèn)題。曹雨等[12]通過(guò)利用單應(yīng)性的轉(zhuǎn)遞特性計(jì)算單應(yīng)性矩陣，緩解相機(jī)視野約束對(duì)機(jī)器人運(yùn)動(dòng)的影響。李寶全等[13]通過(guò)單應(yīng)性矩陣的快速分解得到了軌跡跟蹤誤差，并設(shè)計(jì)自適應(yīng)控制律，實(shí)現(xiàn)了機(jī)器人的視覺(jué)伺服軌跡跟蹤控制。

上述視覺(jué)伺服控制方法的研究主要集中于輪式移動(dòng)機(jī)器人，關(guān)于視覺(jué)伺服用于足式移動(dòng)機(jī)器人的研究較少。相較于輪式移動(dòng)機(jī)器人，足式移動(dòng)機(jī)器人具備更優(yōu)秀的地形適應(yīng)能力，在崎嶇路面、多障礙物路面擁有更高的通過(guò)率。引入視覺(jué)控制技術(shù)，使足式移動(dòng)機(jī)器人能夠獲取更豐富的環(huán)境信息，具備對(duì)跟蹤目標(biāo)的能力，提高足式移動(dòng)機(jī)器人在非結(jié)構(gòu)化環(huán)境中的智能型和自主決策能力。然而，足式移動(dòng)機(jī)器人足式機(jī)構(gòu)的運(yùn)動(dòng)形式較為復(fù)雜，周期性的步態(tài)運(yùn)動(dòng)易對(duì)視覺(jué)伺服控制精度造成不利影響。因此，足式移動(dòng)機(jī)器人的視覺(jué)伺服控制方法仍有待深入研究。

在先前的工作中，提出了一種新型足式機(jī)構(gòu)，并設(shè)計(jì)了用于地形探索的八足移動(dòng)機(jī)器人[14]。視覺(jué)伺服控制技術(shù)是實(shí)現(xiàn)該目標(biāo)的基礎(chǔ)，本文提出一種基于單應(yīng)性矩陣的視覺(jué)伺服控制方案，在缺乏目標(biāo)深度信息的情況下利用視覺(jué)反饋實(shí)現(xiàn)足式移動(dòng)機(jī)器人的控制目標(biāo)，并引入足式機(jī)構(gòu)運(yùn)動(dòng)學(xué)，提高控制系統(tǒng)對(duì)機(jī)器人的控制精度。通過(guò)分析機(jī)器人行走機(jī)構(gòu)運(yùn)動(dòng)學(xué)，建立實(shí)現(xiàn)該機(jī)器人視覺(jué)伺服控制所需的速度映射模型；在機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析結(jié)果的基礎(chǔ)上，提出一種改進(jìn)型自適應(yīng)中值濾波器，以減小機(jī)構(gòu)的周期運(yùn)動(dòng)對(duì)視覺(jué)反饋過(guò)程的影響；基于機(jī)器人位姿誤差模型，設(shè)計(jì)滑模控制器，完成視覺(jué)伺服控制系統(tǒng)閉環(huán)。最后，利用四自由度移動(dòng)平臺(tái)和搭建系統(tǒng)仿真實(shí)驗(yàn)平臺(tái)，分別驗(yàn)證了自適應(yīng)濾波器和控制方法的有效性。

1 系統(tǒng)建模

為實(shí)現(xiàn)足式機(jī)器人的視覺(jué)伺服控制，本節(jié)首先建立機(jī)器人運(yùn)動(dòng)學(xué)模型，獲得機(jī)器人移動(dòng)速度與機(jī)構(gòu)輸入速度的映射關(guān)系；通過(guò)構(gòu)建機(jī)器人位姿與期望位姿的映射關(guān)系，推導(dǎo)用于控制器設(shè)計(jì)的位姿誤差模型，并利用單應(yīng)性矩陣實(shí)現(xiàn)對(duì)機(jī)器人位姿的估計(jì)，完成伺服控制系統(tǒng)視覺(jué)反饋。

1.1 機(jī)器人運(yùn)動(dòng)學(xué)分析

足式移動(dòng)機(jī)器人的三維模型如圖1所示，該機(jī)器人的兩組四足行走機(jī)構(gòu)關(guān)于平面w1對(duì)稱(chēng)布置。每組機(jī)構(gòu)包含兩個(gè)雙足單元，且由同一個(gè)電機(jī)-圓柱齒輪傳動(dòng)系統(tǒng)驅(qū)動(dòng)。雙足單元中兩腿的機(jī)構(gòu)共用同一個(gè)機(jī)架，且機(jī)構(gòu)曲柄相位差配置為π。

圖1 足式移動(dòng)機(jī)器人三維模型

當(dāng)四足行走機(jī)構(gòu)在水平路面行進(jìn)時(shí)，雙足單元的兩腿依次輪換完成支撐推進(jìn)和邁步跨越動(dòng)作，使四足單元產(chǎn)生相對(duì)地面的移動(dòng)速度vLMR?？紤]到機(jī)器人整體結(jié)構(gòu)的對(duì)稱(chēng)性，通過(guò)控制兩側(cè)四足行走機(jī)構(gòu)的驅(qū)動(dòng)電機(jī)轉(zhuǎn)速即可調(diào)整vLMR，進(jìn)而實(shí)現(xiàn)對(duì)足式移動(dòng)機(jī)器人的運(yùn)動(dòng)控制，即機(jī)器人運(yùn)動(dòng)形式為差速驅(qū)動(dòng)。

為獲得機(jī)器人速度映射關(guān)系，建立的連體坐標(biāo)系如圖2所示Kr。

圖2 機(jī)器人連體坐標(biāo)系

將單目相機(jī)固定在機(jī)器人上，并令相機(jī)坐標(biāo)系與Kr系 重合，則機(jī)器人機(jī)架參考點(diǎn)O的速度與兩側(cè)四足行走機(jī)構(gòu)輸出速度的映射關(guān)系可表示為:

式中：L為 兩側(cè)四足行走機(jī)構(gòu)的中心距；v和 ω分別為點(diǎn)O線(xiàn)速度和機(jī)器人的角速度；vr（vl）為機(jī)器人左（右）側(cè)四足行走機(jī)構(gòu)的輸出速度vLMR。

四足行走機(jī)構(gòu)的輸出速度vLMR是由電機(jī)-齒輪傳動(dòng)系統(tǒng)驅(qū)動(dòng)四腿同時(shí)運(yùn)動(dòng)形成的，為準(zhǔn)確控制機(jī)器人移動(dòng)速度，需進(jìn)一步分析四足行走機(jī)構(gòu)的運(yùn)動(dòng)學(xué)，建立機(jī)器人速度v和 ω與驅(qū)動(dòng)電機(jī)輸出轉(zhuǎn)速的映射關(guān)系?？紤]到機(jī)器人八只腿的機(jī)構(gòu)相同，分析四足行走機(jī)構(gòu)的運(yùn)動(dòng)需首先完成單腿的機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析。

機(jī)器人的單腿為1自由度平面六桿機(jī)構(gòu)（見(jiàn)圖3），由曲柄（桿1）、搖桿（桿3）、機(jī)架（桿4）、連桿（桿2和桿5）和輸出桿（桿6）組成。

圖3 六桿機(jī)構(gòu)示意圖

點(diǎn)RA～RF表示各轉(zhuǎn)動(dòng)副中心， φ表示曲柄輸入角度。為建立機(jī)構(gòu)的運(yùn)動(dòng)學(xué)模型，在點(diǎn)RA處建立參考坐標(biāo)系o-xy。借助阿蘇爾桿組法，可將點(diǎn)RC、 點(diǎn)RF及末端運(yùn)動(dòng)輸出點(diǎn)RG在o-xy系下的位置矢量表示為：

式中：rB、rD和rE分別為點(diǎn)RB、點(diǎn)RD和點(diǎn)RE在o-xy系下的位置矢量；lBC、lCD和lEF分別為桿2、桿3和桿5的長(zhǎng)度；lBD、lCE、lCF、lCG和lFG分別為其下標(biāo)所對(duì)應(yīng)兩點(diǎn)之間的距離。

將式（2）與式（3）代入式（4）可得

式中：A=(1-q1-q3q5+q1q3q5)I+(-q2-q4q5-q3q6+q1q4q5+q1q3q6+q2q3q5)M+(-q4q6+q1q4q6+q2q4q5+q2q3q6)M2+(q2q4q6)M3；

B=q3q5I+(q4q5+q3q6)M+q4q6M2；

C=(q1-q1q3q5)I+(q2-q1q4q5-q1q3q6-q2q3q5)M-(q1q4q6+q2q4q5+q2q3q6)M2-(q2q4q6)M3。

注意到： rB=(lABcosφ lABsinφ)T，lAB=‖rB‖， rE=(lAEcosφ lAEsinφ)T，lAE=‖rE‖，故可將式（5）改寫(xiě)為

式中：uφ=(cosφ sinφ)T；rD=(lAD0)T，lAD=‖rD‖。

當(dāng)給定輸入角 φ時(shí)，利用式（6）可求得機(jī)構(gòu)末端點(diǎn) RG的坐標(biāo)。圖4示出了當(dāng) φ=0 ～2π時(shí)，點(diǎn) RG的運(yùn)動(dòng)軌跡（各桿件長(zhǎng)度取值見(jiàn)表1）。

圖4 點(diǎn)RG 的運(yùn)動(dòng)軌跡

表1 六桿機(jī)構(gòu)無(wú)量綱尺度參數(shù)

因雙足單元由同一電機(jī)驅(qū)動(dòng)且單腿機(jī)構(gòu)相同，當(dāng)機(jī)構(gòu)尺度參數(shù)確定后，機(jī)構(gòu)末端點(diǎn)運(yùn)動(dòng)軌跡具有唯一性，故雙足單元空轉(zhuǎn)時(shí)，兩腿末端點(diǎn)的運(yùn)動(dòng)軌跡相同。假設(shè)空轉(zhuǎn)過(guò)程中雙足單元機(jī)架平面保持水平，即支撐相與擺動(dòng)相的兩個(gè)切換點(diǎn)位于同一水平高度，由于雙足單元的曲柄相位差為π，故可確定在點(diǎn) RG運(yùn)動(dòng)軌跡中的兩個(gè)切換點(diǎn) s1和s2，如圖4所示。據(jù)此，結(jié)合式（6）可將雙足單元末端點(diǎn)在支撐相的運(yùn)動(dòng)軌跡表示為：

式中： φ1（ φ2）對(duì)應(yīng)末端點(diǎn) RG位于 s1（s2）時(shí)的輸入角度。當(dāng)取表1所示的機(jī)構(gòu)尺度參數(shù)時(shí)，經(jīng)計(jì)算可得φ1=75°， φ2=255°。

雙足單元在水平路面行走時(shí)，其相對(duì)路面的運(yùn)動(dòng)取決于末端點(diǎn)在支撐相的運(yùn)動(dòng)。取點(diǎn) RA作為機(jī)架參考點(diǎn)，結(jié)合式（7）可計(jì)算出雙足單元行走時(shí)，當(dāng)φ=φ1～φ2，點(diǎn) RA的運(yùn)動(dòng)軌跡如圖5所示。由圖5可知，雙足單元沿水平面的行進(jìn)運(yùn)動(dòng)除了具有平行于地面的運(yùn)動(dòng)外，還包含一個(gè)垂直于地面的周期運(yùn)動(dòng)。

圖5 點(diǎn)RA的運(yùn)動(dòng)軌跡

考慮到四足行走機(jī)構(gòu)的兩個(gè)雙足單元關(guān)于平面w2前后對(duì)陣布置（見(jiàn)圖1b）），若忽略末端點(diǎn)支撐相軌跡不對(duì)稱(chēng)部分對(duì)四足行走機(jī)構(gòu)運(yùn)動(dòng)的影響，四足行走機(jī)構(gòu)的輸出速度vLMR與雙足單元沿平行于地面方向的運(yùn)動(dòng)速度相等。˙迭代計(jì)算流程如圖6所示。

圖6 迭代計(jì)算流程

為提高計(jì)算效率，采用如圖6所示的迭代算法建立機(jī)構(gòu)輸入角速度 φ˙和四足行走機(jī)構(gòu)的輸出速度vLMR的映射關(guān)系，并將其簡(jiǎn)記為

由式（1）和式（8）可建立機(jī)器人速度v 和 ω與兩側(cè)四足行走機(jī)構(gòu)輸入角速度的映射關(guān)系為：

因此，通過(guò)分析四足行走機(jī)構(gòu)運(yùn)動(dòng)學(xué)，可建立機(jī)器人線(xiàn)速度和角速度與兩側(cè)驅(qū)動(dòng)電機(jī)輸出轉(zhuǎn)速的映射關(guān)系，進(jìn)而實(shí)現(xiàn)對(duì)機(jī)器人移動(dòng)速度的準(zhǔn)確控制。

1.2 位姿誤差模型

本節(jié)以機(jī)器人在期望位姿處的連體系為局部參考坐標(biāo)系，描述機(jī)器人在移動(dòng)過(guò)程中的位姿變化，并構(gòu)造用于后續(xù)控制器設(shè)計(jì)的位姿誤差模型。

移動(dòng)過(guò)程中，機(jī)器人當(dāng)前位姿與期望位姿的幾何關(guān)系如圖7所示， K為當(dāng)前位姿處機(jī)器人的連體坐標(biāo)系， K*為期望位姿處機(jī)器人的連體坐標(biāo)系；Πobj為目標(biāo)點(diǎn) Pi（i=1,2,···,N ）所在平面；n*為平面Πobj的法向量（在 K*系中度量）； d*為 K*系原點(diǎn) O*與平面Πobj的距離。

圖7 坐標(biāo)系關(guān)系

由圖7可知，K*系相對(duì)K系的姿態(tài)矩陣和位置

矢量可表示為：

式中θ為 K*系相對(duì) K系的姿態(tài)角。

由圖7可知，點(diǎn) O在K*系中的位置矢量為

結(jié)合 K*系與 K系之間轉(zhuǎn)角的定義，可進(jìn)一步將機(jī)器人移動(dòng)過(guò)程中的位姿記為

式中： ξ1、 ξ2和ξ3為 位姿誤差ξ 的分量。

對(duì)式（14）兩端求導(dǎo)，并結(jié)合式（13）整理可得：

綜上，由式（12）、式（14）和式（15）可知，通過(guò)調(diào)整機(jī)器人的線(xiàn)速度和角速度，使位姿誤差趨于零，從而控制足式移動(dòng)機(jī)器人逼近期望位姿。

1.3 單應(yīng)性矩陣的計(jì)算與分解

為獲得機(jī)器人位姿誤差ξ，首先需要獲取機(jī)器人當(dāng)前位姿 q。本文利用單應(yīng)性矩陣中的元素構(gòu)造狀態(tài)變量，實(shí)現(xiàn)對(duì)當(dāng)前位姿 q的估計(jì)。單應(yīng)性是指對(duì)一組共面點(diǎn)在不同位姿下拍攝得到的圖像間所具有的幾何轉(zhuǎn)換關(guān)系，而表示其轉(zhuǎn)換關(guān)系的矩陣即為單應(yīng)性矩陣[16]。對(duì)于圖7中一組共面的目標(biāo)點(diǎn) Pi，其在單目相機(jī)成像平面內(nèi)對(duì)應(yīng)像點(diǎn)的像素坐標(biāo) pi，可采用SURF、ORB等[17-18]成熟算法識(shí)別和提取。將機(jī)器人處于期望位姿時(shí)由單目相機(jī)捕獲的圖像作為參考圖像，令 p*i表示目標(biāo)點(diǎn) Pi在參考圖像中對(duì)應(yīng)像點(diǎn)的像素坐標(biāo)，由圖像單應(yīng)性原理[3-4]可知， pi與 p*i的映射關(guān)系為

式中： H 為單應(yīng)性矩陣； K為相機(jī)的內(nèi)部參數(shù)矩陣。

假設(shè)機(jī)器人期望位姿固定，且令 X*軸垂直于平面 Πobj，即n*=(1 0 0)T，由式（10）和式（16）可得到單應(yīng)性矩陣 H的表達(dá)式：

根據(jù)式（17）可建立如下等式約束：

此外， T 與 T*的關(guān)聯(lián)關(guān)系為[16]

利用式（18）和式（19），可得到機(jī)器人相對(duì) K*系的位姿矢量 q，進(jìn)而結(jié)合式（15），即可計(jì)算出位姿誤差ξ，實(shí)現(xiàn)基于單應(yīng)性矩陣的機(jī)器人位姿信息反饋。

2 改進(jìn)型自適應(yīng)中值濾波器

由機(jī)器人運(yùn)動(dòng)學(xué)分析可知，四足行走機(jī)構(gòu)沿水平面移動(dòng)時(shí)，除了具有平行于水平面的運(yùn)動(dòng)外，還包含一個(gè)垂直于水平面的周期運(yùn)動(dòng)。該運(yùn)動(dòng)會(huì)使單目相機(jī)捕獲的圖像出現(xiàn)偏移和旋轉(zhuǎn)，直接對(duì)單應(yīng)性矩陣的計(jì)算產(chǎn)生有界干擾，進(jìn)而影響位姿誤差解算的準(zhǔn)確性。結(jié)合機(jī)器人運(yùn)動(dòng)學(xué)分析結(jié)果，本節(jié)提出一種改進(jìn)型自適應(yīng)中值濾波器，以減小該周期運(yùn)動(dòng)對(duì)機(jī)器人位姿估計(jì)過(guò)程的影響，濾波計(jì)算流程如圖8 所示。

圖8改進(jìn)型自適應(yīng)中值濾波流程

圖8 中的符號(hào)定義如下：(upvp)T為圖像中某一點(diǎn)像素的坐標(biāo)；SW×SW為滑動(dòng)窗口尺寸；Smax×Smax為 滑動(dòng)窗口允許的最大尺寸； Smin×Smin為滑動(dòng)窗口允許的最小尺寸； Zuv為(upvp)T位置的灰度值； Zmin、 Zmed、 Zmax分別為當(dāng)前滑動(dòng)窗口內(nèi)所有像素灰度值的最小值、中值、最大值；g (up,vp+Δvp)為考慮機(jī)器人運(yùn)動(dòng)學(xué)后修正的濾波輸出結(jié)果，即像素坐標(biāo)( upvp+Δvp)處 的灰度值，其中Δ vP被定義為

式中： W 為圖像寬度； f為相機(jī)焦距； Δyr（ Δyl）為輸入角度為 φr（ φl(shuí)） 時(shí)，由式（7）計(jì)算得到的末端點(diǎn) RG與點(diǎn)s1在 坐標(biāo)系o -xy中沿y 軸方向的距離。

改進(jìn)型自適應(yīng)中值濾波利用式（20）引入雙足單元運(yùn)動(dòng)學(xué)分析結(jié)果，在成像平面內(nèi)修正圖像。在利用圖像計(jì)算單應(yīng)性矩陣前，使用改進(jìn)型自適應(yīng)中值濾波器對(duì)圖像進(jìn)行預(yù)處理，降低足式移動(dòng)機(jī)器人運(yùn)動(dòng)對(duì)成像過(guò)程的干擾，進(jìn)而保證后續(xù)利用單應(yīng)性矩陣估計(jì)機(jī)器人位姿的準(zhǔn)確性。

3 控制方案設(shè)計(jì)

選擇機(jī)器人位姿誤差作為控制系統(tǒng)的輸入狀態(tài)變量，結(jié)合前文建立的系統(tǒng)模型和改進(jìn)后的自適應(yīng)濾波器，設(shè)計(jì)整個(gè)視覺(jué)伺服控制系統(tǒng)控制流程如圖9所示。

圖9 控制系統(tǒng)框圖

首先給定期望圖像，結(jié)合當(dāng)機(jī)器人在初始位姿處由單目相機(jī)獲得的圖像判斷當(dāng)前機(jī)器人航向角朝向；根據(jù)航向角朝向情況選擇對(duì)應(yīng)的控制器（1或2），輸出機(jī)器人線(xiàn)速度 v和角速度 ω；再利用式（9）計(jì)算出兩側(cè)四足行走機(jī)構(gòu)的輸入角速度和，根據(jù)機(jī)構(gòu)齒輪組傳動(dòng)比可進(jìn)一步得到兩側(cè)電機(jī)的轉(zhuǎn)速，驅(qū)動(dòng)機(jī)器人移動(dòng)；利用改進(jìn)型中值濾波器對(duì)捕獲的圖像進(jìn)行預(yù)處理，結(jié)合期望圖像計(jì)算單應(yīng)性矩陣，進(jìn)而得到位姿誤差，構(gòu)成控制系統(tǒng)閉環(huán)，實(shí)現(xiàn)對(duì)足式機(jī)器人的反饋控制。

機(jī)器人航向角的朝向可分為圖10所示的4種情況。對(duì)于第1種和第4種情況，即‖θ‖＜‖ψ‖，使用控制器1控制機(jī)器人原地轉(zhuǎn)動(dòng)，直至不等式‖θ‖＞‖ψ‖成 立；對(duì)于第2種和第3種情況，即‖θ‖≥‖ψ‖，使用控制器2驅(qū)動(dòng)機(jī)器人由當(dāng)前位姿向期望位姿移動(dòng)，完成控制目標(biāo)。

圖10 機(jī)器人航向角朝向情況

控制器2為基于機(jī)器人位姿誤差的機(jī)器人速度滑?？刂破鳎蕴岣邫C(jī)器人由當(dāng)前位姿趨近期望位姿過(guò)程中控制系統(tǒng)的魯棒性。定義控制器滑模面為：

式中： sv和 sω分別為線(xiàn)速度控制器和角速度控制器所對(duì)應(yīng)的滑模面； c1為正實(shí)數(shù)。

令滑模趨近率為：

式中： ε0、 k0和k1均為正實(shí)數(shù)。

由式（15）、式（21）和式（22）可得速度控制器：

根據(jù)式（23），選定合適的控制器參數(shù) ε0、 c1、k1和k2后，控制器可利用位姿誤差生成機(jī)器人線(xiàn)速度和角速度，再結(jié)足式機(jī)器人運(yùn)動(dòng)學(xué)分析結(jié)果，可進(jìn)一步得到兩側(cè)電機(jī)的輸出速度，實(shí)現(xiàn)對(duì)機(jī)器人運(yùn)動(dòng)的準(zhǔn)確控制。為保證控制器能夠使機(jī)器人最終移動(dòng)到期望位姿，即3個(gè)位姿誤差變量 ξ1、ξ2、ξ3全部趨向0，故構(gòu)造李雅普諾夫函數(shù)對(duì)控制系統(tǒng)穩(wěn)定性進(jìn)行證明，其表述式為

對(duì)式（24）兩端求導(dǎo)，并將式（15）和式（23）代入求導(dǎo)后的公式中，經(jīng)整理可得：

根據(jù)圖7示出的 K 系與 K*系之間的關(guān)系可知Tx*≤0 ，Ty*sinθ ≥0，再由式（14）計(jì)算可得ξ1≤0；考慮到，使用滑模控制器控制機(jī)器人時(shí)， θ 滿(mǎn)足‖θ‖≥‖ψ‖，因而有不等式 ξ1ξ2sgn（ξ3）≥0成立。故通過(guò)限制ε0、 c1、k1和k2的參數(shù)取值，保證不等式c1k1＜ε0+k0成立，可使不等式V˙ ≤0成立且等號(hào)不恒成立，即可保證控制系統(tǒng)的全局穩(wěn)定性。

4 實(shí)驗(yàn)與仿真驗(yàn)證

為驗(yàn)證上述方法的可行性與有效性，本節(jié)開(kāi)展了基于四自由度移動(dòng)平臺(tái)的濾波器測(cè)試實(shí)驗(yàn)和基于CoppeliaSim 軟件的控制系統(tǒng)虛擬仿真，分別驗(yàn)證所提出的改進(jìn)型自適應(yīng)中值濾波器和視覺(jué)伺服控制方法的有效性。

4.1 改進(jìn)型自適應(yīng)濾波算法驗(yàn)證

利用四自由度移動(dòng)平臺(tái)搭載單目相機(jī)，測(cè)試改進(jìn)型自適應(yīng)濾波算法，實(shí)驗(yàn)設(shè)備如圖11所示。使用上位機(jī)可以設(shè)計(jì)移動(dòng)平臺(tái)各個(gè)軸的運(yùn)動(dòng)軌跡，并利用控制器驅(qū)動(dòng)各個(gè)軸的伺服電機(jī)，實(shí)現(xiàn)平臺(tái)運(yùn)動(dòng)，相機(jī)能夠獲取平臺(tái)運(yùn)動(dòng)過(guò)程目標(biāo)對(duì)象的實(shí)時(shí)圖像。

圖11 四自由度移動(dòng)平臺(tái)

利用式（7）可對(duì)式（20）中的 Δyr和Δyl進(jìn)行求解，當(dāng)機(jī)器人兩側(cè)四足行走機(jī)構(gòu)的輸入角度相位差變化時(shí)， Δyr和Δyl會(huì)令機(jī)器人機(jī)架平面產(chǎn)生的3類(lèi)運(yùn)動(dòng)如圖12所示，即垂直于地面的平移運(yùn)動(dòng)、繞機(jī)器人連體系 Kr中 Xr軸的翻滾運(yùn)動(dòng)和側(cè)翻運(yùn)動(dòng)。與這3類(lèi)運(yùn)動(dòng)對(duì)應(yīng)的 Δyr與Δyl的運(yùn)動(dòng)軌跡如圖13所示。

圖12 機(jī)架平面運(yùn)動(dòng)

圖13 Δyr 和Δyl 的運(yùn)動(dòng)軌跡

利用四自由度移動(dòng)平臺(tái)模擬機(jī)架平面的3類(lèi)運(yùn)動(dòng)，以帶有6個(gè)紅色圓形圖案的圖片為目標(biāo)，保存平臺(tái)運(yùn)動(dòng)過(guò)程中的相機(jī)捕獲的圖像。當(dāng)移動(dòng)平臺(tái)處于Δyr=0和 Δyl=0對(duì)應(yīng)的位置時(shí)，將此時(shí)相機(jī)拍攝的圖像選為參考圖像。使用本文提出的改進(jìn)型自適應(yīng)中值濾波器對(duì)保存的圖像進(jìn)行處理，結(jié)合參考圖像，利用1.3節(jié)中單應(yīng)性矩陣分解方法計(jì)算相機(jī)位姿。實(shí)驗(yàn)結(jié)果如圖14所示。

圖14 濾波算法測(cè)試結(jié)果

由圖14可知，機(jī)架平面翻滾和側(cè)翻運(yùn)動(dòng)顯著影響了位姿解算的精度，利用改進(jìn)型中值濾波器能夠有效降低這兩類(lèi)運(yùn)動(dòng)的干擾，提高位姿解算精度。濾波后，Tx*、Ty*和θ 與期望值的誤差為±5 mm、±10 mm和±1°。

綜上所述，改進(jìn)型自適應(yīng)中值濾波器通過(guò)引入四足行走機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析結(jié)果，可以有效減小周期運(yùn)動(dòng)對(duì)機(jī)器人位姿解算過(guò)程的影響，提高位姿解算精度。

4.2 控制方法仿真驗(yàn)證

為驗(yàn)證本文提出控制方案的有效性，本節(jié)采用仿真測(cè)試控制系統(tǒng)性能，利用機(jī)器人仿真軟件CoppeliaSim 構(gòu)建足式機(jī)器人虛擬模型，并搭建仿真場(chǎng)景模擬機(jī)器人運(yùn)動(dòng)情況。CoppeliaSim 是一款動(dòng)力學(xué)仿真軟件，利用軟件提供的內(nèi)嵌工具和物理引擎，可以快速靈活地搭建機(jī)器人仿真場(chǎng)景；借助RemoteAPI，可以采用C++語(yǔ)言實(shí)現(xiàn)控制器，實(shí)時(shí)控制軟件中的虛擬模型。

仿真環(huán)境中相關(guān)設(shè)置如圖15所示， Kg為仿真場(chǎng)景中的全局坐標(biāo)系；足式機(jī)器人虛擬模型初始位置設(shè)置為(0 0．3 m 0．15 m)T，初始航向角偏差設(shè)置為25°；選擇一張帶有6個(gè)紅色圓形的圖片作為目標(biāo)對(duì)象，并將圖片放置于(1.4 m 0 0．15 m)T處；在(1 m 0 0．15 m)T處添加一個(gè)視覺(jué)傳感器，提供目標(biāo)對(duì)象的參考圖像，同時(shí)將該位置設(shè)定為機(jī)器人的期望位置。

圖15 機(jī)器人仿真場(chǎng)景

根據(jù)表1和表2中的機(jī)器人尺度參數(shù)以及表3中相機(jī)參數(shù)和控制器參數(shù)，完成對(duì)控制系統(tǒng)的初始化配置。利用本文提出的控制方案控制機(jī)器人移動(dòng)，測(cè)量并采集機(jī)器人移動(dòng)過(guò)程中相對(duì)Kg系的位姿。

表2 機(jī)器人尺度參數(shù)

表3 視覺(jué)傳感器及控制器參數(shù)

機(jī)器人在仿真場(chǎng)景中的移動(dòng)過(guò)程和視覺(jué)傳感器捕獲的圖像如圖16所示，仿真過(guò)程中機(jī)器人位姿軌跡采樣結(jié)果如圖17所示。由圖17可知，相較于未考慮足式機(jī)構(gòu)運(yùn)動(dòng)的控制系統(tǒng)，本文提出的控制方案能夠更準(zhǔn)確地使機(jī)器人沿理論軌跡移動(dòng)到期望位置， Xg、 Yg和θ的實(shí)際軌跡與理論軌跡的均方根誤差分別為0.028 m、0.012 m 和1.437°，與期望值的誤差最終分別為0.013 m、-0.004 m 和1.067°。

圖16 仿真中機(jī)器人的移動(dòng)過(guò)程

圖17 機(jī)器人位姿軌跡

綜上所述，通過(guò)引入足式機(jī)構(gòu)運(yùn)動(dòng)學(xué)，能夠有效提高足式移動(dòng)機(jī)器人視覺(jué)伺服控制系統(tǒng)性能，保證位姿收斂精度。

5 結(jié)論

本文針對(duì)一種足式移動(dòng)機(jī)器人，提出了基于單應(yīng)性的視覺(jué)伺服控制方案。

1）完成了足式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)分析，通過(guò)建立機(jī)器人移動(dòng)速度與電機(jī)輸出轉(zhuǎn)速間的映射關(guān)系，使控制系統(tǒng)可以更精確地調(diào)整機(jī)器人運(yùn)動(dòng)。

2）為減小機(jī)器人四足行走機(jī)構(gòu)豎直方向運(yùn)動(dòng)對(duì)視覺(jué)反饋環(huán)節(jié)的影響，提出了一種改進(jìn)型自適應(yīng)中值濾波器，使用四自由度移動(dòng)平臺(tái)開(kāi)展實(shí)驗(yàn)，證明了該濾波器可以保障機(jī)器人位姿的估計(jì)精度。

3）通過(guò)設(shè)計(jì)機(jī)器人速度滑?？刂破魍瓿闪俗闶揭苿?dòng)機(jī)器人的視覺(jué)伺服控制方案，并利用仿真實(shí)驗(yàn)證明了本文所提出的控制方法可以更準(zhǔn)確地控制足式移動(dòng)機(jī)器人平穩(wěn)移動(dòng)到期望位姿。

基于本文提出的視覺(jué)伺服控制方案，我們接下來(lái)會(huì)將視覺(jué)控制技術(shù)用于足式機(jī)器人編隊(duì)控制，使機(jī)器人在全局坐標(biāo)不可測(cè)的環(huán)境中，僅利用圖像信息定位自身位置，形成靈活的編隊(duì)結(jié)構(gòu)，完成探索任務(wù)。