基于滾動(dòng)時(shí)域估計(jì)方法的智能小車循跡控制

薛 鵬，任鵬飛

（河南工程學(xué)院 電氣信息工程系，河南 鄭州 451191）

0 引 言

隨著智能控制技術(shù)的發(fā)展，其研究成果廣泛應(yīng)用于諸如家庭、醫(yī)療、救災(zāi)等許多領(lǐng)域。在這些應(yīng)用中，有些需要人工操作機(jī)器進(jìn)行控制，還有些由機(jī)器自動(dòng)完成作業(yè)任務(wù)。智能技術(shù)的不斷成熟及應(yīng)用減輕了工人的勞動(dòng)強(qiáng)度，在危險(xiǎn)或不便由人員參與的惡劣環(huán)境中更可以替代工人工作。以智能車為依托的智能控制技術(shù)已經(jīng)成為該領(lǐng)域內(nèi)的一個(gè)研究熱點(diǎn)[1-3]。其中，智能車路徑跟蹤技術(shù)又是智能控制研究的一個(gè)重要分支。許多文獻(xiàn)設(shè)計(jì)實(shí)現(xiàn)了小車對(duì)預(yù)設(shè)道路的準(zhǔn)確跟蹤[4-5]，而對(duì)循跡跟蹤精度及其快速性的研究更是得到了廣泛的關(guān)注[6]。

模糊策略等許多優(yōu)化控制方法被用來(lái)優(yōu)化提高小車循跡的準(zhǔn)確性。為了在保障準(zhǔn)確性的基礎(chǔ)上優(yōu)化小車的循跡速度，實(shí)現(xiàn)在線處理并優(yōu)化控制量，本研究基于模型預(yù)測(cè)的滾動(dòng)優(yōu)化原理，以智能車的路徑跟蹤問(wèn)題作為研究對(duì)象，闡述目標(biāo)道路信息的獲取、處理、識(shí)別過(guò)程。

具有機(jī)器視覺(jué)的智能車路徑跟蹤控制系統(tǒng)能夠更全面地獲取道路信息，利用各種有效圖像處理算法對(duì)路徑進(jìn)行識(shí)別，提高路徑識(shí)別的準(zhǔn)確性。由于該系統(tǒng)拓寬了智能車的視野，更能對(duì)更遠(yuǎn)的路徑提前做好判斷，提高了路徑跟蹤的平均速度。目標(biāo)道路信息由安裝在小車頂部的CCD攝像頭獲取[7-8]，研究者基于視覺(jué)圖像來(lái)進(jìn)行分析識(shí)別，并在此基礎(chǔ)上運(yùn)用有效的控制算法對(duì)智能小車進(jìn)行控制，對(duì)小車的轉(zhuǎn)向和速度進(jìn)行調(diào)整，使得智能小車能準(zhǔn)確快速地對(duì)道路進(jìn)行跟蹤。

基于滾動(dòng)優(yōu)化原理的滾動(dòng)時(shí)域估計(jì)方法隨著模型預(yù)測(cè)控制研究的不斷深入得到廣泛關(guān)注[9]。它的基本思想是將估計(jì)問(wèn)題轉(zhuǎn)化為固定時(shí)長(zhǎng)的優(yōu)化問(wèn)題，簡(jiǎn)化了計(jì)算，使得在線處理系統(tǒng)約束問(wèn)題成為可能。

本研究首先建立智能小車的非線性約束動(dòng)態(tài)數(shù)學(xué)模型，然后基于滾動(dòng)優(yōu)化時(shí)域估計(jì)方法提出智能小車循跡控制策略。

1 智能小車的數(shù)學(xué)模型

本研究首先建立智能小車的動(dòng)態(tài)數(shù)學(xué)模型，如圖1所示。

圖1 小車動(dòng)態(tài)數(shù)學(xué)模型

根據(jù)以上參數(shù)，該小車的數(shù)學(xué)模型為：

為了方便對(duì)小車進(jìn)行數(shù)字化智能控制，筆者利用前向差分近似計(jì)算方法，并取采樣時(shí)間為T(mén)，可得到小車系統(tǒng)式（1）的離散數(shù)學(xué)模型為：

接下來(lái)分析系統(tǒng)的狀態(tài)空間表達(dá)式。

由于：

應(yīng)用三角函數(shù)加法定理，并引入?yún)?shù)：η1=s i nθ，η2=c o sθ，α1(ω)=c o s(Tω)，α2(w)=s i n(Tω)，由離散系統(tǒng)式（2），可得：

進(jìn)一步取系統(tǒng)變量：

可得到智能小車離散模型的狀態(tài)空間表達(dá)式為：

其中：

在系統(tǒng)的狀態(tài)方程式（7）中，輸入控制量為小車前進(jìn)速度v和轉(zhuǎn)動(dòng)角速度ω。顯然，行列式A與速度v和角速度ω有關(guān)。若假設(shè)小車前進(jìn)速度v恒定，則只有轉(zhuǎn)動(dòng)角速度為控制變量，此時(shí)行列式A僅與角速度ω有關(guān)，狀態(tài)空間模型可用下式表示：

2 滾動(dòng)時(shí)域估計(jì)控制策略

卡爾曼濾波常被用來(lái)設(shè)計(jì)估計(jì)器，但是，在存在約束條件的目標(biāo)道路跟蹤問(wèn)題中，卡爾曼濾波方法往往難以達(dá)到期望精度，會(huì)導(dǎo)致估計(jì)結(jié)果與實(shí)際情況不符[10]。

本研究采用滾動(dòng)時(shí)域優(yōu)化估計(jì)方法來(lái)解決小車對(duì)目標(biāo)道路的跟蹤優(yōu)化問(wèn)題。這種方法具有處理約束和滾動(dòng)優(yōu)化的特點(diǎn)，能夠?qū)⒐烙?jì)問(wèn)題轉(zhuǎn)化為帶約束的優(yōu)化問(wèn)題，從而使問(wèn)題得以解決[11]。另外，滾動(dòng)時(shí)域估計(jì)算法還避免了計(jì)算量隨時(shí)間不斷增大的缺點(diǎn)，使得在線處理優(yōu)化問(wèn)題成為可能。

根據(jù)滾動(dòng)時(shí)域估計(jì)方法的基本思想，系統(tǒng)的預(yù)定跟蹤目標(biāo)設(shè)定為r，預(yù)測(cè)區(qū)間長(zhǎng)度為N，系統(tǒng)輸出的預(yù)測(cè)輸出值為y(k+1)，y(k+2)，…,y(k+N)。本研究將當(dāng)前時(shí)刻開(kāi)始的N個(gè)控制輸入量記為u(k)，u(k+1)，…,u(k+N-1)。這些值同時(shí)計(jì)算得到，但是只有當(dāng)前控制輸入u(k)被用于系統(tǒng)的實(shí)際控制。每一步的控制量的計(jì)算方法都一樣。計(jì)算輸入控制量的目標(biāo)是為了使預(yù)測(cè)區(qū)間內(nèi)的預(yù)測(cè)輸出值接近目標(biāo)值。這種優(yōu)化方法被稱為滾動(dòng)時(shí)域估計(jì)方法。

坐標(biāo)系的選擇和濾波算法是目標(biāo)跟蹤研究領(lǐng)域的兩個(gè)主要問(wèn)題。本研究?jī)H考慮智能小車實(shí)現(xiàn)平面跟蹤的情形。在時(shí)刻k，系統(tǒng)（1）的輸出，即小車位置為：

跟蹤的目標(biāo)路線位置為：

式中：Xwl,Ywl—廣域坐標(biāo)系中目標(biāo)道路位置的坐標(biāo)。

在離散模型式（9）中，系統(tǒng)在時(shí)刻k,k+1,…,k+N-1的對(duì)應(yīng)輸入量為wi,i=1,2,…,N，相應(yīng)的預(yù)測(cè)輸出為：

因此，系統(tǒng)狀態(tài)方程為：

其中，小車轉(zhuǎn)動(dòng)角速度ω需要滿足約束：

其中，約束條件的參數(shù)D,δ由小車轉(zhuǎn)動(dòng)電機(jī)的性能決定。

為了使得輸出預(yù)測(cè)值與目標(biāo)值之間的誤差達(dá)到最小，實(shí)現(xiàn)智能小車對(duì)目標(biāo)道路的精確跟蹤，本研究引入目標(biāo)函數(shù)：

本研究假設(shè)系統(tǒng)的估計(jì)初值x(0)是零均值的正態(tài)分布變量，求解具有約束條件（14）的最小優(yōu)化問(wèn)題：

即可得到系統(tǒng)優(yōu)化輸入列：

進(jìn)而實(shí)現(xiàn)小車與目標(biāo)道路之間誤差的最小化。

3 目標(biāo)道路的獲取方法

智能小車追蹤的目標(biāo)是目標(biāo)道路的位置，下面對(duì)目標(biāo)道路的獲取方法進(jìn)行說(shuō)明。

該試驗(yàn)中，目標(biāo)道路的位置不是預(yù)先給定的。研究者利用固定在小車頂部的攝像機(jī)采集道路信息，并以攝像機(jī)采集到的圖像的其左上角為原點(diǎn)建立屏幕坐標(biāo)系Os-xsys，此時(shí)，目標(biāo)道路的位置坐標(biāo)為(xs l,ys l)。再換個(gè)角度，從三維空間中俯瞰的角度來(lái)分析要跟蹤的目標(biāo)道路。

行走路線的攝像頭坐標(biāo)如圖2所示。

圖2 行走路線的攝像頭坐標(biāo)

本研究以攝像頭所在位置為坐標(biāo)原點(diǎn)，建立三維坐標(biāo)系Oc-XcYcZc，在這個(gè)坐標(biāo)系中，原來(lái)的路線坐標(biāo)(xs l,ys l)被重新定義為(Xc l,Yc l,Zc l)，二者之間的關(guān)系為：

其中：

式中：Xoff，Yoff，Zoff，WD，WS—攝像機(jī)硬件參數(shù)；d—視覺(jué)誤差值，這個(gè)值越大表示被拍攝物體離鏡頭越近，相反地，它越小就表示被拍攝物體離鏡頭越遠(yuǎn)；Yc l—攝像機(jī)與路線地面的高度差。

該試驗(yàn)中，假定行車道路是平坦的，即Yc l=c為給定常數(shù)，那么，道路位置為：

在式（20）中，目標(biāo)道路的坐標(biāo)表示其在三維坐標(biāo)系Oc-XcYcZc中位置。因?yàn)樵撟鴺?biāo)系是以小車為原點(diǎn)建立的，目標(biāo)道路的位置并非其絕對(duì)坐標(biāo)，而是以小車為基準(zhǔn)的相對(duì)位置坐標(biāo)。接下來(lái)，筆者設(shè)法將坐標(biāo)(Xc l,Yc l)轉(zhuǎn)換成廣域坐標(biāo)系中的道路位置(Xwl,Ywl)。廣域坐標(biāo)系是指三維空間中以任意點(diǎn)為坐標(biāo)原點(diǎn)的坐標(biāo)系。該試驗(yàn)中，筆者取小車的出發(fā)點(diǎn)為原點(diǎn)建立廣域坐標(biāo)系Ow-XwYw，如圖2所示。那么，二者之間的變換公式為：

式中：Xwl,Ywl—廣域坐標(biāo)系中目標(biāo)道路的位置；X,Y—由式（9）求得的小車位置坐標(biāo)；θv—兩個(gè)坐標(biāo)系坐標(biāo)軸之間的夾角。從攝像頭坐標(biāo)到廣域坐標(biāo)的變換圖如圖3所示。

圖3 從攝像頭坐標(biāo)到廣域坐標(biāo)的變換

式（11）中的目標(biāo)道路位置坐標(biāo)值通過(guò)式（21）在小車行進(jìn)過(guò)程中依次計(jì)算獲取。

4 試驗(yàn)結(jié)果

試驗(yàn)中，用方向盤(pán)控制小車的轉(zhuǎn)角w。方向盤(pán)轉(zhuǎn)角u和小車轉(zhuǎn)動(dòng)角度的關(guān)系可由試驗(yàn)數(shù)據(jù)得到，即：ω=0.2754u。

假設(shè)小車勻速前進(jìn)，設(shè)定速度v=150mm/s。受約束條件式（14）的限制，方向盤(pán)的最大轉(zhuǎn)動(dòng)角度選取u1=-30°和u2=30°兩個(gè)值。此時(shí)，系統(tǒng)行列式為：

目標(biāo)道路坐標(biāo)值：

試驗(yàn)小車跟蹤其前方約25 cm處開(kāi)始的目標(biāo)道路，其結(jié)果如圖4所示。

圖4 小車行走軌跡和預(yù)測(cè)目標(biāo)道路位置

輸出預(yù)測(cè)值，即小車預(yù)定位置坐標(biāo)由式（22，23），取N=3，按照式（12）計(jì)算得到。

由最小化目標(biāo)函數(shù)式（17）計(jì)算小車方向角的控制輸入量，結(jié)果如圖5所示。

圖5 各采樣時(shí)刻方向盤(pán)轉(zhuǎn)角控制量

從圖4中可以看出，小車的行進(jìn)軌跡與識(shí)別路徑相一致，實(shí)現(xiàn)了對(duì)預(yù)定路線的跟蹤控制。需要注意的是圖4中小車軌跡的起始位置和停車位置和識(shí)別路徑有誤差，這是由于攝像機(jī)安裝在車頂，其采集得到的目標(biāo)道路必然超前于小車本身的位置。

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

針對(duì)搭載攝像頭的智能小車，本研究建立了小車的俯視平面動(dòng)態(tài)模型，并基于滾動(dòng)時(shí)域控制方法提出了對(duì)目標(biāo)道路跟蹤的優(yōu)化控制策略。通過(guò)攝像頭采集道路信息具有更大的信息獲取量，拓寬了智能小車的視野，提高了其路徑識(shí)別的準(zhǔn)確性。而基于滾動(dòng)時(shí)域估計(jì)的預(yù)測(cè)控制方法，將估計(jì)問(wèn)題轉(zhuǎn)化為固定時(shí)長(zhǎng)的優(yōu)化問(wèn)題，也提高了其對(duì)目標(biāo)道路預(yù)測(cè)的快速性。

試驗(yàn)結(jié)果表明，該方案對(duì)小車實(shí)現(xiàn)智能目標(biāo)道路跟蹤是合理可行的，并且具有較高的跟蹤精度。但是試驗(yàn)中的目標(biāo)道路僅僅是一段平滑圓弧，并未對(duì)急轉(zhuǎn)彎等極端情形下小車的跟蹤性能進(jìn)行考察，這將是接下來(lái)的研究?jī)?nèi)容；另外，小車行走過(guò)程中的平滑度問(wèn)題也需在今后的研究中進(jìn)一步深入討論。

致 謝

該試驗(yàn)中部分?jǐn)?shù)據(jù)的采集得到了日本金澤大學(xué)山本實(shí)驗(yàn)室成員的大力協(xié)助，在此向他們表示衷心的感謝。

（References）：

[1] 盧韶芳，劉大維.自主式移動(dòng)機(jī)器人導(dǎo)航研究現(xiàn)狀及其相關(guān)技術(shù)[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，2002，1（2）：112-116.

[2] 尉成果，賴 歡，王太江.輪式移動(dòng)機(jī)器人軌跡跟蹤控制算法的研究[J].機(jī)電工程，2012，29（6）：730-732.

[3] LAMIRAUX F，LAUMON J P.Smooth motion planning for car-like vehicles[J].IEEE Transactions on Robotics And Automation，2001，17（4）：498-502.

[4] 董雷剛，崔曉微，張 丹.智能循跡電動(dòng)小車的設(shè)計(jì)[J].大慶師范學(xué)院學(xué)報(bào)，2010，30（6）：8-10.

[5] 龍鴻儒，何漢武，胡兆勇,等.虛擬環(huán)境下基于非水平路面的智能自主汽車運(yùn)動(dòng)研究[J].機(jī)電工程技術(shù)，2011，40（8）：13-15.

[6] 許曉飛，夏培容.智能車速度的模糊控制最優(yōu)循跡策略研究[J].北京信息科技大學(xué)學(xué)報(bào)，2009，24（3）：84-86.

[7] 劉 崧，戚小平，鐘雙英.CCD攝像機(jī)原理及應(yīng)用[J].中國(guó)有線電視，2005（14）：1417-1419.

[8] 張?jiān)浦蓿瑤煻髁x，吳成東.基于CCD的智能車導(dǎo)航系統(tǒng)研究[J].東北大學(xué)學(xué)報(bào)：自然科學(xué)版，2009，30（2）：162-165.

[9] 王 曌，劉志遠(yuǎn)，裴 潤(rùn).一種滾動(dòng)時(shí)域估計(jì)方法在目標(biāo)跟蹤中的應(yīng)用[J].機(jī)器人，2001，23（7）：594-597.

[10] WANG Xiao-fei，ZOU Zao-jian，WANG Yang，et al.Path following control of underactuated ships based on unscent?ed Kalman filter[J].Journal of Shanghai Jiaotong Uni?versity（Science），2010，15（1）：108-113.

[11] 趙海艷，陳 虹.噪聲方差不確定約束的滾動(dòng)時(shí)域估計(jì)[J].控制與決策，2008，23（2）：217-220.