孟 靜 ，高曉丁 ，陳 罡

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

0 引 言

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

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

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

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

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

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

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

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

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

它們?cè)谶\(yùn)動(dòng)學(xué)模型中是控制輸入，移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)方程為:

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

位姿誤差微分方程為:

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

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

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

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

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

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

由式(4，5)可得:

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

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

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

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

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

1．3 軌跡信息的獲取

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

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

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

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

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

2 道路模型

2．1 連續(xù)路段

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

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

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

當(dāng)移動(dòng)機(jī)器人在軌跡跟蹤過(guò)程中出現(xiàn)如圖2 所示的非連續(xù)路段時(shí)，需先將攝像頭采集到的圖像進(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 的像素個(gè)數(shù)為mj，則整幅圖的像素總數(shù)為:

圖2 非連續(xù)路段

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

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

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

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

并且有P1+P2=1。

從而這兩類(lèi)的均值分別為:

這兩類(lèi)的方差分別為:

這樣就可以得到圖像的類(lèi)間方差為:

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

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

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

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

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

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

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

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

3 軌跡跟蹤

3．1 軌跡跟蹤控制原理

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

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

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

圖4 軌跡跟蹤控制框圖

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

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

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

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

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

4 仿真試驗(yàn)

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

圖5 算法程序流程圖

圖6 直線(xiàn)軌跡跟蹤效果圖

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

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

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

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

圖10 位姿誤差變化曲線(xiàn)

5 結(jié)束語(yǔ)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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