非連續(xù)路段下移動機(jī)器人的軌跡跟蹤

孟 靜 ，高曉丁 ，陳 罡

(1．西安工程大學(xué)機(jī)電工程學(xué)院，陜西西安710000;2．浙江紡織服裝學(xué)院機(jī)電學(xué)院，浙江寧波315200)

0 引 言

基于視覺的移動機(jī)器人軌跡跟蹤已成為研究熱點，目前對移動機(jī)器人的軌跡跟蹤多是連續(xù)路段。如在自動化生產(chǎn)車間里，機(jī)器人沿著固定軌跡完成搬運(yùn)貨物等重復(fù)性的工作;在醫(yī)院里機(jī)器人充當(dāng)傳送病歷及常用資料的傳遞員等［1］。文獻(xiàn)［2］基于Backstepping 思想設(shè)計了一種滑模變結(jié)構(gòu)軌跡跟蹤控制系統(tǒng)，很好實現(xiàn)了移動機(jī)器人的軌跡跟蹤。文獻(xiàn)［3］采用Lyapunov 函數(shù)直接法提出了一種軌跡跟蹤控制算法，分析了系統(tǒng)的漸進(jìn)穩(wěn)定性。對于移動機(jī)器人在非連續(xù)中的運(yùn)動情況，目前尚未有系統(tǒng)的研究，本研究就輪式移動機(jī)器人在非連續(xù)路段中如何實現(xiàn)精確的軌跡控制等問題進(jìn)行討論。

對于路面狀況的分析，目前多是針對于道路裂縫等問題進(jìn)行研究，本研究在裂縫的識別與算法的基礎(chǔ)上，對非連續(xù)路段問題進(jìn)行深入分析與研究，通過合理的算法實現(xiàn)對移動機(jī)器人在非連續(xù)路段中的穩(wěn)定控制。

1 運(yùn)動學(xué)模型及目標(biāo)信息的獲取

1．1 運(yùn)動學(xué)模型

筆者研究的輪式移動機(jī)器人采用兩個驅(qū)動輪、一個萬向輪的結(jié)構(gòu)。左、右兩個驅(qū)動輪安裝在同一軸線上，分別安裝兩個電機(jī)驅(qū)動機(jī)器人運(yùn)動，當(dāng)兩個電機(jī)的轉(zhuǎn)動速度不同時，左、右兩輪將會產(chǎn)生差動，則可實現(xiàn)移動機(jī)器人的轉(zhuǎn)彎動作［4］。由于萬向輪僅起支撐作用，故忽略其在運(yùn)動學(xué)模型中的作用，建立的移動機(jī)器人的運(yùn)動模型如圖1 所示。

圖1 運(yùn)動學(xué)模型

機(jī)器人的位姿可由它中心所在點的坐標(biāo)及方向角表示，令:

Pc=(xc，yc，θc)T，q=(v，w)T

式中:(xc，yc)T—移動機(jī)器人的位置;θc—移動機(jī)器人前進(jìn)方向與x 軸的夾角;v，w—移動機(jī)器人的線速度和角速度。

它們在運(yùn)動學(xué)模型中是控制輸入，移動機(jī)器人的運(yùn)動學(xué)方程為:

本研究中的軌跡跟蹤是讓機(jī)器人沿著給定路徑行走，當(dāng)機(jī)器人從當(dāng)前位姿Pc=(xc，yc，θc)T移動到目標(biāo)位姿Pr=(xr，yr，θr)T時，產(chǎn)生了一個新的坐標(biāo)系xe－ye，令移動機(jī)器人的位姿誤差為Pe=(xe，ye，θe)T，其中，θe=θr－θc。設(shè)新坐標(biāo)系xe－ye與原坐標(biāo)系x－y之間的夾角即為θc，可以得到機(jī)器人位姿誤差方程:

位姿誤差微分方程為:

由以上分析可知，移動機(jī)器人運(yùn)動學(xué)模型的軌跡跟蹤可以定義為尋找有界輸入v 和w，使對任意初始誤差，位姿誤差微分方程在該控制輸入作用下有界且=0。

1．2 攝像機(jī)的標(biāo)定

攝像機(jī)的標(biāo)定方法一般是在攝像機(jī)前放置一個立體標(biāo)靶，用上面位置已知的點作為特征點，采集標(biāo)靶圖像后，利用這些點的圖像坐標(biāo)和三維空間坐標(biāo)建立方程，解方程即可獲得攝像機(jī)的內(nèi)、外參數(shù)［5］。本研究采用Faugeras 的線性模型攝像機(jī)標(biāo)定法對攝像機(jī)進(jìn)行標(biāo)定。設(shè)點的世界坐標(biāo)系已知，攝像機(jī)的內(nèi)參數(shù)模型為:

式中:Min—攝像機(jī)的內(nèi)參數(shù)矩陣;(xc，yc，zc)—景物點在攝像機(jī)坐標(biāo)系下的坐標(biāo);(u，v)—景物點的圖像坐標(biāo)。

攝像機(jī)的外參數(shù)模型為:

式中:(xw，yw，zw)—景物點在世界坐標(biāo)系下的坐標(biāo)且已知;cMw—外參數(shù)矩陣。

由式(4，5)可得:

將式(6)展開并消掉zc后，得下式:

由于空間的每個點都符合式(7)兩個方程，于是可得式(8)所示矩陣:

將外參數(shù)矩陣cMw和M 矩陣寫成式(9)形式，可得式(10)所示矩陣:

cMw中的R 是單位正交矩陣。利用單位正交矩陣的性質(zhì)，可以從M 矩陣中分解出攝像機(jī)的內(nèi)外參數(shù)，如下式:

求出以上參數(shù)即可完成對攝像機(jī)的標(biāo)定。

1．3 軌跡信息的獲取

本研究采用雙目立體視覺系統(tǒng)確定空間目標(biāo)點相對于攝像機(jī)的三維空間位姿。設(shè)兩臺攝像機(jī)C1和C2的內(nèi)參數(shù)采用四參數(shù)模型，相對外參數(shù)用c1Mc2表示?？臻g點P 在攝像機(jī)C1中的圖像坐標(biāo)為(u1，v1)T，則像點坐標(biāo)P1c1表示為:

空間點P 在攝像機(jī)C1光軸中心點與點P1c1構(gòu)成的直線上，即符合方程:

將點P1c2在C2中的坐標(biāo)轉(zhuǎn)換為在C1中的坐標(biāo)，如下式:

同理可求得點P 在C2中的像點坐標(biāo)以及方程

式(16)中Px、Py和Pz構(gòu)成c1Mc2的位置偏移量。直線方程(13，16)的交點即為空間點P。至此可以求出空間點的坐標(biāo)，同理可以通過判定其他點的位置獲取目標(biāo)軌跡［6-7］。

2 道路模型

2．1 連續(xù)路段

當(dāng)為連續(xù)路段時，本研究選取直線模型y =kx +b作為軌跡模型，然后利用路徑線上的n 個像素點(假設(shè)像素點坐標(biāo)為(ai，ci)其中n ＞1，1≤i≤n)和最小二乘法對路徑線模型的參數(shù)k 和b 進(jìn)行擬合計算［8］:

最后，根據(jù)計算出的直線模型即可確定移動機(jī)器人的期望跟蹤軌跡。

2．2 非連續(xù)路段

當(dāng)移動機(jī)器人在軌跡跟蹤過程中出現(xiàn)如圖2 所示的非連續(xù)路段時，需先將攝像頭采集到的圖像進(jìn)行圖像預(yù)處理，突出圖像中的非連續(xù)路段，再運(yùn)用二值化的方法進(jìn)行圖像區(qū)域分割，提取出圖像中非連續(xù)路段信息［9-10］。

二值化方法的關(guān)鍵在于閾值的選取與設(shè)定，本研究主要采用大津法進(jìn)行閾值的確定，該方法的原理如下:

設(shè)圖2 中圖像的灰度值集合為P={0，1，2，3，…，255}，灰度值為j 的像素個數(shù)為mj，則整幅圖的像素總數(shù)為:

圖2 非連續(xù)路段

則對應(yīng)灰度值為j 的像素概率分布為:

式中:j∈P，Qj≥0，Qj=1。

則整幅圖像的灰度平均值μ，方差σ2可表示為:

假若整幅圖像以灰度值作為閾值，那么將圖像分割成為P1={0，1，…T}，P2={T +1，T +2，…255}兩個類Z1，Z2，此時，類Z1和類Z2發(fā)生的概率P1，P2，可以表述為:

并且有P1+P2=1。

從而這兩類的均值分別為:

這兩類的方差分別為:

這樣就可以得到圖像的類間方差為:

圖3 非連續(xù)路段的二值化圖像

在檢測到非連續(xù)路段后，還需計算非連續(xù)路段的寬度，看其是否超出機(jī)器人行走的安全范圍，計算方法為［11］:

(1)求出二值非連續(xù)路段圖像水平投影Y。

(2)設(shè)在坐標(biāo)點j 處的投影值為pro［j］，pro［j］大于0。

(3)遍歷二值非連續(xù)路段細(xì)化圖像，選取j(j，y)點及其相鄰點i(i，x)，計算此處裂縫的傾角α。

(4)計算裂縫寬度值W［j］。

(5)對各個投影點為橫坐標(biāo)的非連續(xù)路段點處的寬度都進(jìn)行上述計算，最大值即為非連續(xù)路段點的最大寬度。

當(dāng)所計算出的非連續(xù)路段點的寬度在機(jī)器人行走的安全范圍內(nèi)時，機(jī)器人按照所給的軌跡繼續(xù)追蹤，當(dāng)超出機(jī)器人行走的安全范圍時，機(jī)器人將停止或后退。

3 軌跡跟蹤

3．1 軌跡跟蹤控制原理

在機(jī)器人的運(yùn)動過程中，可通過機(jī)器人的幾何中心坐標(biāo)，生成實際運(yùn)動軌跡曲線，并比較實際運(yùn)動軌跡與理論運(yùn)動軌跡的差值。當(dāng)二者誤差小于θα?xí)r，不需要對機(jī)器人的運(yùn)動進(jìn)行糾正;當(dāng)二者的誤差超出θα?xí)r，則可根據(jù)設(shè)定的算法程序，控制機(jī)器人左、右電機(jī)以不同的速度轉(zhuǎn)動，完成對機(jī)器人運(yùn)動位姿的糾正與誤差補(bǔ)償，從而實現(xiàn)對機(jī)器人運(yùn)動的閉環(huán)控制與自動跟蹤。經(jīng)過反復(fù)控制器調(diào)節(jié)，最終使得系統(tǒng)位姿誤差Pe趨于零。移動機(jī)器人的軌跡跟蹤框圖如圖4 所示。

3．2 控制器的設(shè)計

為了使移動機(jī)器人的跟蹤誤差快速收斂于零，獲得良好的軌跡跟蹤效果，本研究設(shè)計了基于Lyapunov函數(shù)的控制器［12-14］，其控制律如下:

圖4 軌跡跟蹤控制框圖

令控制參數(shù)a1，a2，aθ有界，且都大于零。

構(gòu)造Lyapunov 函數(shù)為:

結(jié)合式(2，3)對式(34)求導(dǎo)得:

將式(33)代入式(35)得:

根據(jù)Lyapunov 穩(wěn)定性判據(jù)可知，當(dāng)a1，a2，aθ有界且都大于零時，用式(33)表示的控制率能使輪式移動機(jī)器人位姿誤差趨近于零。

4 仿真試驗

為了驗證控制算法的有效性，本研究用Matlab 進(jìn)行算法編程，對機(jī)器人直線軌跡跟蹤的連續(xù)路段和非連續(xù)路段進(jìn)行仿真［15-16］。算法程序流程圖如圖5 所示。參考軌跡的初始位置為xr(0)=0，yr(0)=0，θr(0)=π/4 控制輸入為v=0．2 m/s，w=0，角度誤差θα=7°，移動機(jī)器人的初始位置為xc(0)=1．5，yc(0)=1．5，θc(0)=2π/3;控制參數(shù)a1=a2=10，aθ=50，裂縫寬度的安全值為W［j］=5 mm，SV=2;直線軌跡跟蹤效果圖如圖6 所示;當(dāng)為連續(xù)路段時，仿真結(jié)果如圖7 所示。當(dāng)t=3．2 s 出現(xiàn)非連續(xù)路段時，非連續(xù)路段寬度安全范圍如圖8 所示，非安全范圍如圖9 所示。

圖5 算法程序流程圖

圖6 直線軌跡跟蹤效果圖

圖7 連續(xù)路段軌跡跟蹤效果圖

圖8 非連續(xù)可走路段軌跡跟蹤效果圖

移動機(jī)器人的跟蹤結(jié)果與偏差變化如圖6 所示。機(jī)器人在連續(xù)路段中的軌跡跟蹤情況如圖7 所示;圖8 中，機(jī)器人在t=3．1 s 時遇到非連續(xù)路段，其寬度小于5 mm，故機(jī)器人可以行走，但在此刻位移誤差出現(xiàn)了波動，在t=10 s 時實現(xiàn)了穩(wěn)定跟蹤，收斂到理想軌跡;圖9 中在t=3．2 s 時遇到非連續(xù)路段，其寬度大于5 mm，機(jī)器人不可以行走，故最終停留原地;圖10 中在t=2．5 s 時位姿誤差收斂到零，從而驗證了所設(shè)計的控制率能夠?qū)崿F(xiàn)移動機(jī)器人對給定軌跡的跟蹤。

圖9 非連續(xù)不可走路段軌跡跟蹤效果圖

圖10 位姿誤差變化曲線

5 結(jié)束語

本研究討論了移動機(jī)器人在非連續(xù)路段下的軌跡跟蹤問題，利用二值化方法對非連續(xù)路段圖像進(jìn)行處理，通過大津法對閾值進(jìn)行確定來提取圖像的非連續(xù)路段信息，并設(shè)計了基于Lyapunov 函數(shù)的控制器實現(xiàn)了機(jī)器人對給定軌跡的跟蹤，最后用Matlab 進(jìn)行仿真，仿真結(jié)果證明了該控制率及算法的可行性。通過對非連續(xù)路段的處理，使機(jī)器人能夠更好地實現(xiàn)對道路軌跡的跟蹤，防止機(jī)器人在運(yùn)行過程中因路面的破損而損壞機(jī)器人。

與其他算法進(jìn)行比較，大津法能夠選取出理想的閾值，但在很多情況下不是最佳的分割，在后續(xù)內(nèi)容中，本研究將著重于最佳閾值的分割進(jìn)行研究。

［1］日本機(jī)器人學(xué)會．機(jī)器人技術(shù)手冊(新版)［M］．宗光華，稱君實譯．北京:科學(xué)出版社．2008．

［2］GREYY W，PONDAR． The URANUS mobile robot［R］．The Robotics Institute Technical Report． Carnegie-Mellon University，1986:127-129．

［3］胡浚濤，李亞榮，李 林．輪式移動機(jī)器人軌跡跟蹤控制研究［J］．儀表技術(shù)，2013(4):27-28．

［4］上官望義．輪式移動機(jī)器人移動性能研究及樣機(jī)設(shè)計開發(fā)［D］．西安:西安理工大學(xué)機(jī)儀學(xué)院，2009．

［5］王寶磊，王朝立，李 菲，等．基于雙目視覺伺服反饋的非完整移動機(jī)器人軌跡跟蹤［J］． 測控自動化，2012，28(10):125．

［6］張顯偉，吳忠偉，張鳳海，等．移動機(jī)器人模糊滑模軌跡跟蹤控制［J］．長春工業(yè)大學(xué)，2013，34(4):374-375．

［7］曹 洋，項龍江，徐心和．基于全局視覺的輪式移動機(jī)器人軌跡跟蹤控制［J］．機(jī)器人，2004，26(1):79-82．

［8］李晉蕙． 用圖像處理的方法檢測公路路面裂縫類病害［J］．長安大學(xué)學(xué)報:自然科學(xué)版，2004，24(3):25-29．

［9］肖旺新，張 雪，黃 衛(wèi)，等．路面破損自動識別的一種新算法［J］．公路交通科技，2005，22(11):75-78．

［10］孫波成，邱延峻． 基于圖像處理的路面裂縫識別研究［J］．重慶交通大學(xué)學(xué)報:自然科學(xué)版，2008，27(1):62-64．

［11］肖 靖．道路裂縫識別算法研究［D］．北京:北京郵電大學(xué)理學(xué)院，2009．

［12］劉 磊，向 平，王永驥，等．非完整約束下的輪式移動機(jī)器人軌跡跟蹤［J］．清華大學(xué)學(xué)報:自然科學(xué)版，2007，47(S2):1885-1889．

［13］岳龍旺，朱敬花．基于輪式移動平臺的新型濕式清潔機(jī)器人系統(tǒng)［J］．輕工機(jī)械，2013，31(6):66-68．

［14］羅 劍，雷 勇．智能移動機(jī)器人中視覺跟蹤算法研究［J］．儀器儀表學(xué)報，2009，30(6):336-340．

［15］晏祖根，李 明，徐克非，等．高速機(jī)器人分揀系統(tǒng)機(jī)器視覺技術(shù)的研究［J］．包裝與食品機(jī)械，2014(1):28-31．

［16］劉金琨．機(jī)器人控制系統(tǒng)的設(shè)計與Matlab 仿真［M］．北京:清華大學(xué)出版社，2008．