丁夏清,杜卓洋,陸逸卿,劉　山

(浙江大學(xué) 控制科學(xué)與工程學(xué)院,浙江 杭州 310027)

?

基于混合勢場的移動(dòng)機(jī)器人視覺軌跡規(guī)劃

丁夏清,杜卓洋,陸逸卿,劉山

(浙江大學(xué) 控制科學(xué)與工程學(xué)院,浙江 杭州 310027)

摘要:針對載有單目相機(jī)的非完整移動(dòng)機(jī)器人,提出基于混合人工勢場的能夠滿足非完整約束的路徑規(guī)劃方法,能夠解決移動(dòng)機(jī)器人運(yùn)行過程中的視野約束問題.在傳統(tǒng)人工勢場的基礎(chǔ)上,提出混合人工勢場的概念,勢場中一部分是只對角速度起作用的目標(biāo)點(diǎn)偏轉(zhuǎn)勢場和視野約束排斥勢場,另一部分是同時(shí)影響線速度和角速度的目標(biāo)位姿吸引勢場；其中視野約束排斥勢場定義在圖像空間內(nèi)；目標(biāo)位姿吸引勢場和目標(biāo)點(diǎn)偏轉(zhuǎn)勢場定義在笛卡爾空間,利用單應(yīng)性矩陣三維重建的結(jié)果構(gòu)造勢場函數(shù).在混合人工勢場的控制下,機(jī)器人能夠在同時(shí)滿足視野約束和無側(cè)滑約束的條件下平滑地移動(dòng)到目標(biāo)位姿.仿真結(jié)果證明了該方法的有效性.

關(guān)鍵詞：移動(dòng)機(jī)器人；非完整約束；視野約束；人工勢場；單應(yīng)性矩陣

在移動(dòng)機(jī)器人視覺伺服控制策略的研究中,視野約束(field-of-view constrains,FOV)和機(jī)器人運(yùn)動(dòng)的非完整約束是需要解決的兩個(gè)重要問題[1]. 視野約束問題是指機(jī)器人利用視覺信息進(jìn)行反饋控制時(shí),控制策略須保證目標(biāo)維持在視野范圍內(nèi)；當(dāng)目標(biāo)移出視野范圍時(shí),控制策略仍能保證機(jī)器人移動(dòng)到正確的位姿.針對該問題,目前主要的解決方案有基于切換控制的視覺伺服策略[2]、基于預(yù)測控制的視覺伺服策略[3]和結(jié)合軌跡規(guī)劃的視覺伺服策略等,其中軌跡規(guī)劃可以細(xì)分為基于圖像插值的軌跡規(guī)劃[4]、基于優(yōu)化的軌跡規(guī)劃[5-7]和基于勢場的軌跡規(guī)劃等.

基于人工勢場路徑規(guī)劃的方法是機(jī)器人路徑規(guī)劃中的常用方法,基本思想是在機(jī)器人工作空間內(nèi)施加虛擬勢場,并將任務(wù)目標(biāo)設(shè)計(jì)為勢場的極值點(diǎn),使機(jī)器人在勢場力的作用下運(yùn)動(dòng)到目標(biāo)位姿.目前,基于人工勢場法的軌跡規(guī)劃策略多是針對可產(chǎn)生任意方向加速度的機(jī)器人設(shè)計(jì)的,不適用于非完整移動(dòng)機(jī)器人.針對該問題,Borenstein等[8]提出利用合力的大小和方向解耦地控制機(jī)器人的線速度和角速度的方法；朱毅等[9]對人工勢場法進(jìn)行改進(jìn),利用模糊控制的方法,根據(jù)環(huán)境信息調(diào)整控制律中的參數(shù).

在單目相機(jī)視覺系統(tǒng)中,單目相機(jī)無法獲取目標(biāo)深度信息,機(jī)器人的位姿是不完全可測的.很多學(xué)者對于如何從單目相機(jī)中獲取機(jī)器人的位姿信息進(jìn)行了研究.Gonzalo等[2]根據(jù)單應(yīng)性矩陣中的元素設(shè)計(jì)控制器,可以使運(yùn)動(dòng)過程中特征點(diǎn)始終在機(jī)器人視野內(nèi)；Fang等[10]基于單應(yīng)性矩陣設(shè)計(jì)切換控制器,可以使機(jī)器人以最短路徑運(yùn)動(dòng)到期望位姿處；李寶全等[11]基于二維三焦張量設(shè)計(jì)反饋線性化控制器,實(shí)現(xiàn)了機(jī)器人的位置鎮(zhèn)定.本文引入單應(yīng)性矩陣,通過分解單應(yīng)性矩陣得到機(jī)器人的軌跡規(guī)劃所需的相關(guān)參數(shù),可以避免估計(jì)目標(biāo)深度信息.

本文提出基于混合人工勢場的方法,分別構(gòu)造影響角速度的偏轉(zhuǎn)勢場與視野約束排斥勢場以及同時(shí)影響線速度和角速度的目標(biāo)位姿吸引勢場.其中,偏轉(zhuǎn)勢場和目標(biāo)位姿吸引勢場定義在笛卡爾空間,并利用基于單應(yīng)性矩陣的三維重建結(jié)果構(gòu)造勢場函數(shù)；視野約束的斥力勢場定義在圖像空間,基于特征點(diǎn)到圖像邊緣的距離構(gòu)造勢場函數(shù).本文在笛卡爾空間內(nèi)規(guī)劃機(jī)器人的線速度和角速度,以保證控制律滿足非完整約束,并針對勢場中存在的局部極值的問題給出解決方法.在該控制方法下,機(jī)器人的線速度和角速度連續(xù),并且能夠穩(wěn)定地移動(dòng)到目標(biāo)位姿.通過仿真驗(yàn)證了該方法的有效性.

1問題描述

圖1　差動(dòng)輪式機(jī)器人及目標(biāo)平面示意圖Fig.1　Schematics of differential wheeled robots and target plane

如圖1所示,考慮載有單目相機(jī)的非完整移動(dòng)機(jī)器人的視覺伺服任務(wù),空間中存在平面的目標(biāo)物體,給定機(jī)器人在期望姿態(tài)下相機(jī)拍攝的目標(biāo)物體圖像,利用視覺反饋使機(jī)器人從目標(biāo)可見的任意初始位姿調(diào)整到期望位姿.

對于圖1所示的差動(dòng)輪式機(jī)器人,單目相機(jī)固定在機(jī)器人輪軸的中點(diǎn),相機(jī)光軸與機(jī)器人坐標(biāo)系z軸平行,Fc和F*分別為機(jī)器人在當(dāng)前位姿和期望位姿下的相機(jī)坐標(biāo)系,原點(diǎn)在機(jī)器人輪軸的中心,xz平面定義了機(jī)器人的運(yùn)動(dòng)平面,其中x軸平行于機(jī)器人輪軸線,z軸垂直于機(jī)器人輪軸線.

在機(jī)器人的控制過程中,存在以下兩個(gè)約束.

1)非完整約束：在運(yùn)動(dòng)過程中,輪式機(jī)器人無法側(cè)向滑動(dòng),存在非完整約束.

2)視野約束：在機(jī)器人運(yùn)動(dòng)過程中,須保持目標(biāo)圖像始終在視野內(nèi).

本文提出混合的人工勢場,將人工勢場分成兩部分.一部分是只對角速度起作用的目標(biāo)點(diǎn)偏轉(zhuǎn)勢場和視野約束排斥勢場；另一部分是同時(shí)影響線速度和角速度的目標(biāo)位姿吸引勢場,利用合勢場對機(jī)器人的作用效果在笛卡爾空間中進(jìn)行軌跡規(guī)劃,使機(jī)器人平滑地移動(dòng)到目標(biāo)位姿.

2系統(tǒng)模型

2.1視覺模型

如圖2所示,π為空間中一平面,Mj=[Xi,Yj,Zj]T(j=1,2,…,n,n為特征點(diǎn)總數(shù))為平面上一點(diǎn),n*為在F*下π平面的單位法向量,d*為F*原點(diǎn)到平面π的距離,mj=[xj,yj,1]T為點(diǎn)Mj的歸一化坐標(biāo),其中xj=Xj/Zj,yj=Yj/Zj,pj=[uj,vj,1]T,和p*j=[u*j,v*j,1]T分別為當(dāng)前圖像和目標(biāo)圖像中點(diǎn)Mj的像素齊次坐標(biāo),s=[(p1)T,(p2)T,…,(pn)T]T為特征向量.

圖2　當(dāng)前位姿和期望位姿的相機(jī)坐標(biāo)系Fig.2　Desired and current camera frames

兩幅圖像中共面的特征點(diǎn)可以用一個(gè)映射單應(yīng)性矩陣G關(guān)聯(lián)[12]:

αjpj=Gp*j.

(1)

式中：αj為正比例系數(shù).

G=AHA-1=A(R+tdn*T)A-1.

(2)式中：td=t/d*；H為當(dāng)前圖像和期望圖像間的單應(yīng)性矩陣；R為兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)矩陣；t為兩個(gè)坐標(biāo)系間的平移向量；矩陣A為相機(jī)的內(nèi)參矩陣,

(3)

其中αu、αv為用像素單位表示的相機(jī)焦距長度,αuv為傾斜因子,[u0,v0]為相機(jī)主點(diǎn)坐標(biāo).

假設(shè)目標(biāo)點(diǎn)始終在前方,并且已知法向量(n*)T的方向,可以分解單應(yīng)性矩陣,得到關(guān)于R和td的唯一解[13].

2.2機(jī)器人模型

如圖3所示為差動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型.圖中,v為機(jī)器人瞬時(shí)線速度,ω為瞬時(shí)角速度,ζ為世界坐標(biāo)系與機(jī)器人當(dāng)前坐標(biāo)系的夾角.

圖3　差動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型Fig.3　Kinematic model of wheeled mobile robot

在左、右驅(qū)動(dòng)輪滾動(dòng)無滑動(dòng)的假設(shè)下,機(jī)器人存在如下的非完整約束：

vxcosζ-vzsinζ=0

(4)

式中：vx為機(jī)器人速度在目標(biāo)位姿機(jī)器人坐標(biāo)系F*下x*軸的分量,vz為機(jī)器人速度在F*下z*軸的分量.

3基于混合人工勢場法的軌跡規(guī)劃

人工勢場法的基本思想是在機(jī)器人工作空間內(nèi)施加虛擬人工勢場,并設(shè)計(jì)任務(wù)目標(biāo)為勢場的極值點(diǎn),使機(jī)器人在勢場力的作用下運(yùn)動(dòng)到目標(biāo)位姿.人工勢場包括目標(biāo)對機(jī)器人產(chǎn)生的吸引勢場和障礙物對機(jī)器人產(chǎn)生的排斥勢場,機(jī)器人在合勢場的作用下完成避障并到達(dá)目標(biāo)位姿的任務(wù).

為了使機(jī)器人在人工勢場作用下的運(yùn)動(dòng)滿足視野約束和非完整約束,本文對傳統(tǒng)的人工勢場函數(shù)加以改進(jìn),分別針對機(jī)器人的速度和角速度設(shè)計(jì)了混合人工勢場,其中包括只對角速度起作用的目標(biāo)點(diǎn)偏轉(zhuǎn)勢場Vθ和視野約束排斥勢場Vs以及同時(shí)影響線速度和角速度的目標(biāo)位姿吸引勢場Va.

3.1混合人工勢場模型

3.1.1目標(biāo)位姿產(chǎn)生的吸引勢場在傳統(tǒng)人工勢場中,目標(biāo)對機(jī)器人的吸引勢場多由兩者之間的距離定義得到.考慮到單目相機(jī)無法獲取目標(biāo)的深度信息,即目標(biāo)與機(jī)器人間的距離無法計(jì)算,因此利用單應(yīng)性矩陣分解的結(jié)果進(jìn)行三維重建,構(gòu)造人工勢場中的目標(biāo)位姿吸引勢場函數(shù).

將機(jī)器人位姿的矢量定義為γ=[xd,zd,φ]T,其中xd為單應(yīng)性矩陣分解得到的平移向量td在機(jī)器人坐標(biāo)系X軸上的投影,zd為td在機(jī)器人坐標(biāo)系z軸上的投影,φ為從F系繞y軸順時(shí)針旋轉(zhuǎn)到F*系的角度.目標(biāo)位姿為γ*=[0,0,0]T.

定義目標(biāo)位姿對機(jī)器人的吸引勢場為

(5)

該吸引勢場產(chǎn)生的吸引力為

(6)

目標(biāo)位姿產(chǎn)生的吸引勢場作用在機(jī)器人上,同時(shí)影響線速度和角速度,引導(dǎo)機(jī)器人運(yùn)動(dòng)到目標(biāo)位姿.

對于具有非完整約束的移動(dòng)機(jī)器人,無法瞬時(shí)產(chǎn)生任意方向的加速度,只利用目標(biāo)位姿吸引勢場產(chǎn)生的作用力無法保證機(jī)器人能夠到達(dá)目標(biāo)位姿,機(jī)器人可能最終停止在與目標(biāo)位姿平行的狀態(tài)；在運(yùn)動(dòng)的過程中,無法保證特征點(diǎn)始終不會(huì)脫離視野范圍.本文引入克服非完整約束的偏轉(zhuǎn)勢場和克服視野約束的圖像空間斥力場.由于非完整約束和視野約束可以通過調(diào)整機(jī)器人的前進(jìn)方向解決,為了減少線速度和角速度控制之間的耦合,將偏轉(zhuǎn)勢場和視野約束的斥力勢場定義為只影響機(jī)器人角速度的勢場函數(shù).

3.1.2偏轉(zhuǎn)勢場由于研究的機(jī)器人存在非完整約束,無法側(cè)向滑動(dòng),為了使機(jī)器人能夠運(yùn)動(dòng)到目標(biāo)點(diǎn),且減少非完整約束對吸引勢場的作用效果的影響,應(yīng)使機(jī)器人在運(yùn)動(dòng)過程中的前進(jìn)方向指向目標(biāo)位置.為此,引入影響機(jī)器人角速度的偏轉(zhuǎn)勢場Vθ,將其定義為

(7)

式中：θ為機(jī)器人當(dāng)前位姿與機(jī)器人和目標(biāo)點(diǎn)的連線之間的夾角.

偏轉(zhuǎn)勢場如圖4所示，偏轉(zhuǎn)角越大,機(jī)器人受到的偏轉(zhuǎn)力越大,機(jī)器人朝指向目標(biāo)位姿的方向偏轉(zhuǎn),并且由于勢場函數(shù)中cos2φ的存在,φ和θ會(huì)產(chǎn)生相互約束的作用,機(jī)器人不會(huì)迅速地直接轉(zhuǎn)向目標(biāo)點(diǎn),而是沿著光滑曲線運(yùn)動(dòng),最終到達(dá)目標(biāo)姿態(tài).

利用單應(yīng)性矩陣分解得到的td,計(jì)算θ為

(8)

式中：ε為一較小閾值.

由式(7)可得偏轉(zhuǎn)勢場對機(jī)器人產(chǎn)生的偏轉(zhuǎn)力為

[0,0,2sinθcosθ+cosφsinφ]T.

(9)

圖4　偏轉(zhuǎn)勢場示意圖Fig.4　Illustration for deflection potential field

3.1.3視野約束的排斥勢場為了防止機(jī)器人在運(yùn)動(dòng)過程中目標(biāo)點(diǎn)移出視野范圍,定義了圖像空間中的排斥勢場.當(dāng)特征點(diǎn)距圖像邊緣的距離小于一定值時(shí),斥力勢場的作用將使其向圖像中部運(yùn)動(dòng).排斥勢場函數(shù)的表達(dá)式[12]如下：

(10)

式中：

斥力場函數(shù)示意圖如圖6所示，當(dāng)圖像特征點(diǎn)在工作區(qū)域C內(nèi)時(shí),斥力勢場對機(jī)器人不產(chǎn)生作用力,當(dāng)特征點(diǎn)距圖像邊緣的距離小于邊界寬度α?xí)r,斥力勢場產(chǎn)生作用使特征點(diǎn)向圖像中部運(yùn)動(dòng),并且特征點(diǎn)越靠近圖像邊緣,斥力勢場越大.

當(dāng)視野約束的斥力勢場存在時(shí),由式(10)可得

(11)

M矩陣的表達(dá)式為

(12)

圖5　斥力場示意圖Fig.5　Repulsive potential field for visibility

圖6　斥力場函數(shù)示意圖Fig.6　Repulsive potential function for visibility

(13)

式中：

圖像雅可比矩陣的計(jì)算需已知目標(biāo)深度信息,只憑單目相機(jī)無法準(zhǔn)確獲取深度信息,因此無法直接根據(jù)公式計(jì)算圖像雅克比矩陣和視野約束排斥力.

通過分解單應(yīng)性矩陣得到的R、t/d*和n*,可以計(jì)算出

(14)

利用ρj可以將圖像雅克比矩陣改寫成：

(15)

式中：

(16)

S矩陣和Q矩陣中的元素可以通過圖像信息計(jì)算得到,從而得到包含d*的L矩陣.

L(s,Z)=[LT(p1,Z1),…,LT(pn,Zn)]T.

(17)

Fs(γ)=[0,0,Fsω]T.

(18)

3.2非完整約束下的實(shí)現(xiàn)方法

在不考慮視野約束時(shí),針對具有非完整約束的移動(dòng)機(jī)器人,傳統(tǒng)的基于人工勢場法的2種控制策略[8]如下.

(19)

2) ω=k1β, v=k2F.

(20)

式中：β為機(jī)器人當(dāng)前速度方向與受到的合力間的偏轉(zhuǎn)角,α為斥力方向與當(dāng)前速度方向的夾角,vmax為機(jī)器人最大運(yùn)動(dòng)速度,F和Fγ分別為合力和斥力,k1、k2為控制參數(shù).

這兩種控制方法都存在一些弊端.第一種控制方案無法保證機(jī)器人在目標(biāo)點(diǎn)停止,并且在受到斥力時(shí),速度完全取決于斥力方向,可能會(huì)造成前方?jīng)]有障礙物的情況下斥力方向相互抵消,導(dǎo)致機(jī)器人陷入局部極值無法到達(dá)目標(biāo)點(diǎn)；第二種控制方法在k1和k2選取不恰當(dāng)時(shí)會(huì)造成機(jī)器人的運(yùn)動(dòng)發(fā)散.

本文提出以下的控制策略,能夠有效地解決上述問題,使機(jī)器人最終停止在目標(biāo)位姿.

(21)

式中：δ1、δ2、δ3為每個(gè)勢場的比例因子.

機(jī)器人的瞬時(shí)速度vc和角速度ωc的表達(dá)式為

(22)

3.3勢場的極值問題

在混合勢場中,機(jī)器人可能陷入混合勢場的局部極小值點(diǎn),此時(shí)機(jī)器人受到的合力為0,無法到達(dá)目標(biāo)位姿.由式(22)可知,當(dāng)F2=0,Fω=0時(shí),機(jī)器人陷入混合勢場的局部極值點(diǎn).

在設(shè)計(jì)混合勢場時(shí),為了使規(guī)劃出的軌跡更接近于非完整約束移動(dòng)機(jī)器人運(yùn)動(dòng)的軌跡,當(dāng)偏轉(zhuǎn)角較大時(shí),偏轉(zhuǎn)勢場對角速度的作用效果應(yīng)大于吸引勢場對角速度的作用效果,即須滿足

(23)

當(dāng)機(jī)器人陷入局部極值時(shí),偏轉(zhuǎn)角θ為最大值π/2.在滿足式(23)的前提下,吸引勢場和偏轉(zhuǎn)勢場產(chǎn)生的作用力無法使機(jī)器人平衡,即此時(shí)視野約束的勢場一定已經(jīng)發(fā)生作用,特征點(diǎn)在圖像邊緣.

為了解決局部極值點(diǎn)的問題,當(dāng)機(jī)器人陷入局部極值時(shí),基于圖像信息規(guī)劃了如圖7所示的軌跡,以使機(jī)器人脫離局部極值,并最終正確移動(dòng)到目標(biāo)位姿.

圖7　機(jī)器人跳出局部極值點(diǎn)的軌跡示意圖Fig.7　Special paths for robot to get rid of local minimums

圖8　機(jī)器人跳出局部極值點(diǎn)過程中圖像特征點(diǎn)變化示意圖Fig.8　Illustration of features’ movements during robots getting rid of local minimum

圖8中淺灰度和深灰度部分的定義同圖5,其中黑色圓點(diǎn)表示特征點(diǎn)在相機(jī)圖像平面上的投影.設(shè)目標(biāo)圖像中,垂直方向上坐標(biāo)值最小的特征點(diǎn)為p*min=[u*min,v*min,1]T,坐標(biāo)值最大的特征點(diǎn)為p*max=[u*max,v*max,1]T；設(shè)圖像空間中,特征點(diǎn)在水平和垂直方向上距邊界的最小距離分別為

(24)

(25)

當(dāng)機(jī)器人陷入局部極小值時(shí),圖像平面如圖8的第1幅圖所示,部分特征點(diǎn)處于圖像邊界.此時(shí)保持這部分特征點(diǎn)的橫坐標(biāo)處于圖像邊界處,使機(jī)器人向前運(yùn)動(dòng)直至特征點(diǎn)觸及垂直方向上第1個(gè)中間狀態(tài)設(shè)置的邊界值,則圖像平面如圖8的第2幅圖所示.垂直方向的第1個(gè)中間狀態(tài)定義了特征點(diǎn)在圖像空間垂直方向的邊界值Dvmin1、Dvmax1,

Dvmin1=0.5v*min,

(26)

(27)

則這一階段的速度表達(dá)式為

(28)

ωc=kA·eu.

(29)

該軌跡規(guī)劃的效果是使圖像特征點(diǎn)運(yùn)動(dòng)到與第1個(gè)中間狀態(tài)定義的最近的邊界重合.

Dv2=1.5vmid-v*min.

(30)

該階段的速度表達(dá)式為

vc=kc(vmin-Dv2).

(31)

ωc=0.

(32)

此時(shí),機(jī)器人已經(jīng)退回到目標(biāo)位姿的后方,機(jī)器人可以切換回原有的基于混合勢場的軌跡規(guī)劃策略,圖像平面如圖8的第3幅圖所示.若機(jī)器人再次陷入局部極值點(diǎn),則繼續(xù)利用該策略跳出極值點(diǎn).

4仿真研究

為了驗(yàn)證本文方案的有效性,基于MATLAB/Simulink[15]對所提算法進(jìn)行仿真驗(yàn)證.

不失一般性,設(shè)機(jī)器人的目標(biāo)位姿坐標(biāo)系與世界坐標(biāo)系重合,圖9中的網(wǎng)格區(qū)域?yàn)橄鄼C(jī)可見的視野范圍在圖像平面的投影.設(shè)目標(biāo)平面上提取的特征點(diǎn)有4個(gè),當(dāng)機(jī)器人在初始位姿和目標(biāo)位姿時(shí),特征點(diǎn)在相機(jī)圖像平面上的成像分別用圓點(diǎn)和三角點(diǎn)表示.圖9的3幅圖像表示了機(jī)器人初始時(shí)刻視野約束勢場不產(chǎn)生作用、視野約束勢場產(chǎn)生作用和機(jī)器人處于局部極值點(diǎn)的3種情況下的相機(jī)圖像平面示意圖.

圖9　機(jī)器人位于初始位姿和目標(biāo)位姿時(shí)特征點(diǎn)的成像Fig.9　Features in images taken at initial and desired pose

在滿足初始時(shí)特征點(diǎn)在相機(jī)視野范圍內(nèi)的條件下,機(jī)器人的初始位姿可以任意選取. 根據(jù)實(shí)驗(yàn)結(jié)果調(diào)整參數(shù),以優(yōu)化軌跡和穩(wěn)態(tài)誤差.最終選取的參數(shù)為：δ1=0.32,δ2=19.25,δ3=0.96,kv=0.3,kω=6,k=2.2,km=0.01,kA=0.01,kB=0.01,kc=0.03.

定義圖像空間誤差e為所有特征點(diǎn)像素誤差的和,笛卡爾空間誤差為ed和ea,其中ed為當(dāng)前位姿與目標(biāo)位姿的距離,ea為當(dāng)前位姿與目標(biāo)位姿的夾角.

下文分別通過運(yùn)動(dòng)過程中視野約束斥力場不產(chǎn)生作用力、運(yùn)動(dòng)過程中視野約束斥力場產(chǎn)生作用力和運(yùn)動(dòng)過程中陷入局部極小值并跳出3種情況驗(yàn)證混合勢場的有效性,并通過在第1種情況下進(jìn)行有、無偏轉(zhuǎn)勢場的對比驗(yàn)證引入偏轉(zhuǎn)勢場對路徑規(guī)劃結(jié)果的作用.

4.1運(yùn)動(dòng)過程中視野約束斥力場不產(chǎn)生作用

選取機(jī)器人在世界坐標(biāo)系下的初始位置為[1,0,-3]T,旋轉(zhuǎn)角為0.1rad.在移動(dòng)向目標(biāo)位姿的過程中,特征點(diǎn)沒有運(yùn)動(dòng)到相機(jī)圖像平面的邊緣,視野約束斥力場未產(chǎn)生作用,機(jī)器人的運(yùn)動(dòng)軌跡如圖10的左側(cè)軌跡所示,特征點(diǎn)誤差的收斂曲線如圖11所示.圖10中右側(cè)的運(yùn)動(dòng)軌跡沒有引入偏轉(zhuǎn)勢場的運(yùn)動(dòng)軌跡,對于具有沿輪軸向非完整約束的移動(dòng)機(jī)器人,偏轉(zhuǎn)勢場的引入具有重要作用. 機(jī)器人運(yùn)動(dòng)過程中平移距離和旋轉(zhuǎn)角度的收斂曲線如圖12所示,機(jī)器人運(yùn)動(dòng)過程中速度和角速度的變化曲線如圖13所示.

在改進(jìn)人工勢場的控制下,機(jī)器人能夠沿著較光滑的曲線移動(dòng)向目標(biāo)位姿,運(yùn)動(dòng)過程滿足非完整約束的限制條件,最終特征點(diǎn)的像素誤差可以收斂到零.

圖10　機(jī)器人運(yùn)動(dòng)軌跡示意圖Fig.10　Trajectory of mobile robot

圖11　特征點(diǎn)的像素誤差收斂曲線Fig.11　Pixel error in image space

圖12　機(jī)器人運(yùn)動(dòng)過程中平移距離和旋轉(zhuǎn)角度的收斂曲線Fig.12　Convergence curves of translation error and rotation error during movement

圖13　機(jī)器人運(yùn)動(dòng)過程中速度和角速度的變化曲線Fig.13　Velocity and angular velocity in process of movement

在機(jī)器人運(yùn)動(dòng)過程中,線速度和角速度由式(21)計(jì)算出的合力決定.當(dāng)偏轉(zhuǎn)角θ過大時(shí),混合勢場的控制效果應(yīng)使機(jī)器人快速轉(zhuǎn)向前進(jìn)方向指向目標(biāo)位置,所以開始階段在Fa和Fθ的共同作用下,機(jī)器人迅速調(diào)整前進(jìn)方向,這段過程中角速度較大.

在機(jī)器人逐漸接近目標(biāo)位姿的過程中,td、θ和φ逐漸減小.由式(6)、(9)可知,受到的吸引力和偏轉(zhuǎn)力逐漸減小,即機(jī)器人的線速度和角速度逐漸減小.當(dāng)機(jī)器人到達(dá)目標(biāo)位姿時(shí),合外力中的各分量為零,即機(jī)器人可以停止在目標(biāo)位姿.

4.2運(yùn)動(dòng)過程中視野約束斥力場產(chǎn)生作用

選取機(jī)器人的初始平移向量為[1,0,-2]T,旋轉(zhuǎn)角為0.1rad,在運(yùn)動(dòng)過程中,特征點(diǎn)有移出視野的趨勢,視野約束斥力場產(chǎn)生作用效果.機(jī)器人的運(yùn)動(dòng)軌跡如圖14所示，特征點(diǎn)誤差的收斂曲線如圖15所示，機(jī)器人運(yùn)動(dòng)過程中平移距離和旋轉(zhuǎn)角度的收斂曲線如圖16所示，速度和角速度的變化曲線如圖17所示.

圖14　受視野約束作用時(shí)機(jī)器人在笛卡爾空間內(nèi)的軌跡Fig.14　Moving trajectory in Cartesian space with FOV constraints

圖15　受視野約束下特征點(diǎn)的像素誤差收斂曲線Fig.15　Pixel error in image space with FOV constraints

圖16　受視野約束下機(jī)器人運(yùn)動(dòng)過程中平移距離和旋轉(zhuǎn)角度的收斂曲線Fig.16　Translation error and rotation error with FOV constraints

圖17　受視野約束下機(jī)器人運(yùn)動(dòng)過程中速度和角速度的變化曲線Fig.17　Velocity and angular velocity with FOV constraints

當(dāng)特征點(diǎn)脫離工作空間C時(shí),斥力場產(chǎn)生排斥作用,使機(jī)器人回到安全視野范圍.偏轉(zhuǎn)勢場和吸引勢場對機(jī)器人的運(yùn)動(dòng)產(chǎn)生作用,使機(jī)器人在移向目標(biāo)位姿的同時(shí),前進(jìn)方向指向目標(biāo)位置.此時(shí),這三個(gè)勢場對角速度產(chǎn)生的作用效果相互平衡,特征點(diǎn)維持在圖像平面的邊界處.由于此時(shí)吸引勢場對機(jī)器人的作用效果使機(jī)器人移動(dòng)向目標(biāo)位置,機(jī)器人將沿笛卡爾空間內(nèi)的弧線移動(dòng)向目標(biāo)位姿,偏轉(zhuǎn)角度不斷變化,最終的偏轉(zhuǎn)勢場和吸引勢場的合作用效果不再使特征點(diǎn)有移出工作空間C的趨勢.此時(shí),特征點(diǎn)回到不受視野約束斥力的圖像空間,機(jī)器人此后的運(yùn)動(dòng)與上一部分的運(yùn)動(dòng)相似,最終停止到目標(biāo)位姿.

4.3跳出局部極值

機(jī)器人在陷入局部極值后,將切換到針對該情況的軌跡規(guī)劃方法中.此時(shí),機(jī)器人將向前運(yùn)動(dòng)到第1個(gè)中間狀態(tài),然后向后退到第2個(gè)中間狀態(tài).選取機(jī)器人陷入局部極值時(shí)的位姿如圖18所示，則這一過程中，機(jī)器人的運(yùn)動(dòng)軌跡如圖19所示，機(jī)器人的速度和角速度的變化如圖20所示.在前進(jìn)過程中,機(jī)器人需要維持特征點(diǎn)在圖像空間的邊界處,且此時(shí)機(jī)器人較靠近目標(biāo)點(diǎn),調(diào)整特征點(diǎn)位置所需的角速度較大.當(dāng)機(jī)器人后退到第2個(gè)中間狀態(tài)后,以此時(shí)的狀態(tài)作為新的初始位姿,此后的運(yùn)動(dòng)過程與上文討論的過程相似.

圖18　陷入局部極值時(shí)的機(jī)器人位姿Fig.18　Robot pose with local extremum

圖19　笛卡爾空間內(nèi)機(jī)器人跳出局部極值時(shí)的運(yùn)動(dòng)軌跡Fig.19　Moving trajectory in Cartesian space for robot to get rid of local minimums

圖20　機(jī)器人跳出局部極值的過程中速度和角速度的變化曲線Fig.20　Velocity and angular velocity during robot getting rid of local minimums

5結(jié)語

本文針對載有單目相機(jī)的非完整移動(dòng)機(jī)器人的視覺軌跡規(guī)劃問題,提出基于混合人工勢場的方法,將人工勢場分為以下2個(gè)部分.一部分是只對機(jī)器人角速度起作用的目標(biāo)點(diǎn)偏轉(zhuǎn)勢場和視野約束排斥勢場；另一部分是同時(shí)影響線速度和角速度的目標(biāo)位姿吸引勢場,使機(jī)器人能夠在滿足視野約束和無側(cè)滑約束的條件下運(yùn)動(dòng)到目標(biāo)位姿.當(dāng)陷入局部極值時(shí),引入基于中間狀態(tài)軌跡的規(guī)劃方法,解決勢場局部極小值問題.通過仿真驗(yàn)證了該軌跡規(guī)劃方法的有效性.

參考文獻(xiàn)(References):

[1] 賈丙西,劉山,張凱祥,等. 機(jī)器人視覺伺服研究進(jìn)展：視覺系統(tǒng)與控制策略[J]. 自動(dòng)化學(xué)報(bào),2015,41(5): 861-873．

JIABing-xi,LIUShan,ZHANGKai-xiang,etal.Surveyonrobotvisualservocontrol:visionsystemandcontrolstrategies[J].ActaAutomaticaSinica, 2015, 41(5): 861-873．

[2]GONZALOLN,GANSNR,SOURABHB,etal.Homography-basedcontrolschemeformobilerobotswithnonholonomicandfield-of-viewconstraints[J].IEEETransactionsonSystemsManandCyberneticsPartBCybernetics, 2010, 40(4): 1115-1127．

[3]CAOZC,YINLJ,FUYL,etal.Predictivecontrolforvisualservostabilizationofnonholonomicmobilerobots[J].ActaAutomaticaSinica, 2013, 39(8): 1238-1245．

[4]SCHRAMMF,MORELG.Ensuringvisibilityincalibration-freepathplanningforimage-basedvicsualservo-ing[J].IEEETransactionsonRobotics, 2006, 22(4): 848-854．

[5]CHESIG,PRATTICHIZZOD,VICINOA.Straightlinepathplanninginvisualservoing[J].JournalofDynamicSystemsMeasurementandControlTransactionsoftheASME, 2007, 129(4): 541-543．

[6]CHESIG.VisualservoingpathplanningviahomogeneousformsandLMIoptimizations[J].IEEETransactionsonRobotics, 2009, 25(2): 281-291．

[7]CHESIG,HUNGYS.Globalpath-planningforconstrainedandoptimalvisualservoing[J].IEEETransactionsonRobotics, 2007, 23(5): 1050-1060．

[8]BORENSTEINJ,KORENY.Real-timeobstacleavoidanceforfastmobilerobots[J].IEEETransactionsonSystems,Man,andCybernetics, 1989, 19(5): 1179-1187．

[9] 朱毅,張濤,宋靖雁. 非完整移動(dòng)機(jī)器人的人工勢場法路徑規(guī)劃[J],控制理論與應(yīng)用,2010,27(2): 152-158．

ZHUYi,ZHANGTao,SONGJing-yan.Pathplanningfornonholonomicmobilerobotsusingartificialpotentialfieldmethod[J].ControlTheoryandApplications,2010, 27(2): 152-158．

[10]FANGYong-chun,ZHANGXue-bo.Adaptiveactivevisualservoingofnonholonomicmobilerobots[J].IEEETranscationsonIndustrialElectronics, 2012, 59(1): 486-497．

[11] 李寶全,方勇純,張雪波.基于2D三焦點(diǎn)張量的移動(dòng)機(jī)器人視覺伺服鎮(zhèn)定控制[J],自動(dòng)化學(xué)報(bào),2014,40(12)： 2706-2715．

LIBao-quan,FANGYong-chun,ZHANGXue-bo. 2Dtrifocaltensorbasedvisualservoregulationofnonholonomicmobilerobots[J].ActaAutomaticaSinica, 2014,40(12)： 2706-2715．

[12]MEZOUARY,CHAUMETTEF.Pathplanningforrobustimage-basedcontrol[J].IEEETransactionsonRoboticsandAutomation, 2002, 18(4): 534-549．

[13]ZHANGXue-bo,FANGYong-chun,MABo-jun,etal.Afasthomographydecompositiontechniqueforvisualservoofmobilerobots[C]∥Proceedingsofthe27thChineseControlConference.Beijing:BeihangUniversityPress, 2008: 404-409．

[14]CHAUMETTEF,HUTCHINSONS.Visualservocontrol.I.basicapproaches[J].IEEERoboticsandAutomationMagazine, 2006, 13(4): 82-90．

收稿日期：2015-06-25.浙江大學(xué)學(xué)報(bào)(工學(xué)版)網(wǎng)址： www.journals.zju.edu.cn/eng

基金項(xiàng)目：國家自然科學(xué)基金資助項(xiàng)目(61273133)

作者簡介：丁夏清(1994-)，女，本科生，從事視覺伺服的研究. ORCID：0000-0001-7802-0130.E-mail: xqding@zju.edu.cn 通信聯(lián)系人：劉山，男，副教授.ORCID：0000-0003-1504-341X.E-mail: sliu@iipc.zju.edu.cn

DOI:10.3785/j.issn.1008-973X.2016.07.011

中圖分類號(hào)：TP 242

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1008-973X(2016)07-1298-09

Visual trajectory planning for mobile robots based on hybrid artificial potential field

DING Xia-qing, DU Zhuo-yang, LU Yi-qing, LIU Shan

(CollegeofControlScienceandEngineering,ZhejiangUniversity,Hangzhou310027,China)

Abstract:A visual trajectory planning method was proposed based on hybrid artificial potential field for a nonholonomic mobile robot equipped with a monocular camera in consideration of field-of-view (FOV) constraints. A hybrid model was designed based on the concept of conventional artificial potential field. The model consisted of two parts. One part only affects the angular velocity and involves the terms for target deflection and field of view constraints, and the other part affects both the linear velocity and the angular velocity. The potential field for field of view constraints was defined based on the feature point coordinates in the image space. The potential fields for target deflection and the relative pose were calculated based on the scaled three-dimensional reconstruction results obtained from the decomposition of homography. The robot can be regulated to the desired pose with the targets kept in the field of view under the hybrid artificial potential field. Simulation results were provided in some representative circumstances to show the effectiveness of the proposed method．

Key words:mobile robot; nonholonomic constraint; field of view constraints; artificial potential field; homography matrix