論輪式移動(dòng)結(jié)構(gòu)的非完整約束及其運(yùn)動(dòng)控制1)

周宇生 文相容 王在華

(貴州大學(xué)數(shù)學(xué)與統(tǒng)計(jì)學(xué)院，貴陽(yáng) 550025)

(解放軍陸軍工程大學(xué)基礎(chǔ)部，南京 211101)

引言

輪式移動(dòng)結(jié)構(gòu)是一種通過(guò)驅(qū)動(dòng)輪子轉(zhuǎn)動(dòng)來(lái)實(shí)現(xiàn)運(yùn)動(dòng)功能的經(jīng)典機(jī)械模型，具有能耗小和操作方便等優(yōu)點(diǎn).尤其是獨(dú)輪和兩輪移動(dòng)結(jié)構(gòu)占地小且運(yùn)動(dòng)靈活，能夠?qū)崿F(xiàn)很多復(fù)雜的運(yùn)動(dòng)功能.因此在很多機(jī)械結(jié)構(gòu)中需要加入輪式滾動(dòng)接觸機(jī)構(gòu)，尤其在機(jī)器人、航空航天和機(jī)械制造等領(lǐng)域應(yīng)用十分廣泛.目前在商業(yè)領(lǐng)域中有獨(dú)輪結(jié)構(gòu)Unicycle，兩輪結(jié)構(gòu)Segway PT，三輪結(jié)構(gòu)摩托車，四輪結(jié)構(gòu)汽車和更多輪的拖--掛車.輪式機(jī)器人在近二十年是一個(gè)非常熱門(mén)的研究領(lǐng)域，相關(guān)的建模、路徑規(guī)劃和控制設(shè)計(jì)研究很多[1-3].而在工業(yè)領(lǐng)域，很多公司研制了自己特色的輪式移動(dòng)機(jī)器人[4].2002 年，Grasser 等[5]研制了兩輪式移動(dòng)機(jī)器人Joe，通過(guò)解耦分別驅(qū)動(dòng)控制兩輪轉(zhuǎn)速，使得機(jī)器人的靈活性達(dá)到前所未有的水平.2007 年，德國(guó)Fraunhofer 等研制出一款貨物搬運(yùn)機(jī)器人，該機(jī)器人配備了紅外線傳感器，能到達(dá)指定地點(diǎn)搬運(yùn)貨物.2017 年，谷歌母公司Alphabet 發(fā)布了一款集輪子和腿為一體的機(jī)器人Handle，能夠俯身跳躍和貨物搬運(yùn)，具有非常優(yōu)秀的平衡能力.雖然工業(yè)界研發(fā)的輪式機(jī)器人功能越來(lái)越強(qiáng)大，但這一切在很大程度上應(yīng)歸功于發(fā)達(dá)的測(cè)量技術(shù)和強(qiáng)大的數(shù)據(jù)處理能力.從理論上來(lái)說(shuō)，只要能夠測(cè)量足夠多的精確數(shù)據(jù)，并能很好地處理這些數(shù)據(jù)，就可以不需要實(shí)際模型，而直接基于數(shù)據(jù)驅(qū)動(dòng)設(shè)計(jì)軌跡跟蹤控制器[6].但毫無(wú)疑問(wèn)，這樣做的成本是高昂的，因?yàn)閷?shí)際模型本身就是一種已知信息，放棄這么重要的信息無(wú)疑是一種巨大的浪費(fèi).正確的做法應(yīng)該是盡量利用實(shí)際模型和觀測(cè)信息，結(jié)合現(xiàn)代控制理論設(shè)計(jì)精確高效的低成本控制器.

具有滾動(dòng)接觸的結(jié)構(gòu)在運(yùn)動(dòng)時(shí)會(huì)受到非完整約束[7]，它是對(duì)系統(tǒng)運(yùn)動(dòng)速度的限制，并且不可積.與完整約束限制系統(tǒng)的位形不同，非完整約束是系統(tǒng)狀態(tài)空間上的約束，主要是對(duì)系統(tǒng)運(yùn)動(dòng)速度的限制.在無(wú)任何約束的控制作用下，非完整移動(dòng)結(jié)構(gòu)可以在整個(gè)位形空間內(nèi)運(yùn)動(dòng).因此，一般來(lái)說(shuō)非完整約束的強(qiáng)度要比完整約束要弱，文獻(xiàn)[8]指出，在計(jì)算非完整約束力學(xué)系統(tǒng)的自由度時(shí)，每一個(gè)非完整約束可以使得系統(tǒng)減少0.5 個(gè)自由度.由于非完整約束結(jié)構(gòu)本質(zhì)上還是欠驅(qū)動(dòng)的，導(dǎo)致其路徑規(guī)劃和跟蹤控制設(shè)計(jì)都比較困難[9-10].文獻(xiàn)[11]基于幾何觀點(diǎn)在拉格朗日和哈密頓框架下建立非完整約束力學(xué)系統(tǒng)的動(dòng)力學(xué)方程，并討論了在非完整約束下力學(xué)系統(tǒng)的可控性和控制器設(shè)計(jì)問(wèn)題.文獻(xiàn)[12]提出采用Pontryagin 極大值原理的Hamel 形式來(lái)解決移動(dòng)機(jī)器人在球面上受非完整約束下的最優(yōu)控制問(wèn)題.

要想精確控制輪式移動(dòng)結(jié)構(gòu)的運(yùn)動(dòng)，必須同時(shí)清楚其運(yùn)動(dòng)學(xué)方程和動(dòng)力學(xué)方程，而其動(dòng)力學(xué)方程又需要利用運(yùn)動(dòng)學(xué)方程結(jié)合Euler-Lagrange 方程推導(dǎo)出來(lái).目前很多研究?jī)H僅針對(duì)其運(yùn)動(dòng)學(xué)方程，將約束方程轉(zhuǎn)化為標(biāo)準(zhǔn)鏈?zhǔn)较到y(tǒng)的形式[13]，然后利用相關(guān)鏈?zhǔn)交騼缡较到y(tǒng)的控制理論設(shè)計(jì)速度控制器[14-15].但在實(shí)際問(wèn)題中，實(shí)現(xiàn)運(yùn)動(dòng)任務(wù)所需要的速度是通過(guò)力或力矩來(lái)實(shí)現(xiàn)的，應(yīng)該設(shè)計(jì)力或力矩控制器.另外一些研究?jī)H僅針對(duì)其動(dòng)力學(xué)方程，考慮跟蹤給定的前向速度和偏航轉(zhuǎn)速目標(biāo)，設(shè)計(jì)自適應(yīng)模糊或神經(jīng)網(wǎng)絡(luò)控制實(shí)現(xiàn)給定跟蹤任務(wù)[16-17].但所給定的前向速度和偏航轉(zhuǎn)速目標(biāo)所對(duì)應(yīng)的幾何軌跡是什么并不清楚.而實(shí)際問(wèn)題需要設(shè)計(jì)跟蹤控制使得輪式移動(dòng)結(jié)構(gòu)能夠精確地沿著給定的目標(biāo)軌跡曲線運(yùn)動(dòng).因此，必須同時(shí)考慮其運(yùn)動(dòng)學(xué)方程和動(dòng)力學(xué)方程，設(shè)計(jì)力或力矩控制器使機(jī)器人實(shí)現(xiàn)給定的運(yùn)動(dòng)任務(wù).但是，目前已有的文獻(xiàn)對(duì)輪式非完整約束的運(yùn)動(dòng)規(guī)律理解還不深入，很多研究成果側(cè)重于從控制理論的角度進(jìn)行控制設(shè)計(jì)與分析，很少有研究能將輪式非完整約束下的運(yùn)動(dòng)規(guī)律與機(jī)器人運(yùn)動(dòng)控制設(shè)計(jì)有機(jī)結(jié)合起來(lái).如文獻(xiàn)[18-19]中，將前向速度和偏航轉(zhuǎn)速同時(shí)看成是運(yùn)動(dòng)方程的控制輸入變量和動(dòng)力學(xué)方程的狀態(tài)輸出變量，基于中間變量來(lái)設(shè)計(jì)自適應(yīng)模糊跟蹤控制器.文獻(xiàn)[20]中利用兩個(gè)高增益觀測(cè)器來(lái)估計(jì)兩輪式移動(dòng)結(jié)構(gòu)的前向速度和偏航轉(zhuǎn)速，然后在此基礎(chǔ)上設(shè)計(jì)自適應(yīng)反饋控制器來(lái)實(shí)現(xiàn)圓周運(yùn)動(dòng).文獻(xiàn)[21]采用保辛算法將多體系統(tǒng)連續(xù)時(shí)間域內(nèi)的動(dòng)力學(xué)方程進(jìn)行離散化，再用瞬時(shí)最優(yōu)控制保辛方法實(shí)現(xiàn)對(duì)目標(biāo)軌跡的高精度跟蹤.

為了設(shè)計(jì)控制器使輪式移動(dòng)結(jié)構(gòu)能精確地沿著給定目標(biāo)軌跡曲線運(yùn)動(dòng)，首先應(yīng)分析清楚輪式移動(dòng)結(jié)構(gòu)的非完整約束本質(zhì)，以及機(jī)械結(jié)構(gòu)在輪式非完整約束下的運(yùn)動(dòng)規(guī)律.然后將得到的運(yùn)動(dòng)規(guī)律與動(dòng)力學(xué)方程有機(jī)結(jié)合起來(lái)，再利用現(xiàn)代控制理論設(shè)計(jì)合適的軌跡跟蹤控制器.本文工作從質(zhì)點(diǎn)連續(xù)光滑運(yùn)動(dòng)出發(fā)，認(rèn)清輪式非完整約束的本質(zhì)及理清各類輪式移動(dòng)結(jié)構(gòu)的運(yùn)動(dòng)學(xué)方程，并在此基礎(chǔ)上給出求解動(dòng)力學(xué)方程的一般方法.另一方面，利用平面曲線基本理論分析輪式移動(dòng)結(jié)構(gòu)在非完整約束下的運(yùn)動(dòng)規(guī)律，然后通過(guò)引入相對(duì)曲率為任務(wù)目標(biāo)曲線設(shè)計(jì)動(dòng)態(tài)軌跡跟蹤目標(biāo).基于該動(dòng)態(tài)軌跡跟蹤目標(biāo)，可以將實(shí)際問(wèn)題中的目標(biāo)軌跡曲線、運(yùn)動(dòng)學(xué)方程和動(dòng)力學(xué)方程有機(jī)結(jié)合起來(lái)，為輪式移動(dòng)結(jié)構(gòu)設(shè)計(jì)精確高效的軌跡跟蹤控制器以實(shí)現(xiàn)給定的運(yùn)動(dòng)任務(wù).

1 質(zhì)點(diǎn)沿光滑曲線運(yùn)動(dòng)的特征

1.1 質(zhì)點(diǎn)連續(xù)光滑運(yùn)動(dòng)滿足的條件

如圖1 所示，一個(gè)質(zhì)量為m的帶電質(zhì)點(diǎn)，在電場(chǎng)的作用下在平面上做光滑曲線運(yùn)動(dòng).若要使該質(zhì)點(diǎn)沿給定的光滑曲線運(yùn)動(dòng)，該如何控制電場(chǎng)力才能精確地實(shí)現(xiàn)運(yùn)動(dòng)任務(wù)?這是一個(gè)簡(jiǎn)單的軌跡跟蹤控制問(wèn)題，該質(zhì)點(diǎn)好像沒(méi)有受到任何約束作用，我們直接利用牛頓第二定律就可以得到其動(dòng)力學(xué)方程

其中，Tx,Ty分別是沿坐標(biāo)方向的電場(chǎng)控制力.

假設(shè)給定的光滑曲線是平面上的一個(gè)軌道槽，質(zhì)點(diǎn)在該曲線槽里運(yùn)動(dòng)，那么該質(zhì)點(diǎn)是否受到了約束作用呢?答案是肯定的.此時(shí)該約束是完整約束還是非完整約束呢?注意到質(zhì)點(diǎn)沿該曲線運(yùn)動(dòng)時(shí)，其沿與切向垂直的橫向方向的速度為0，即

圖1 帶電質(zhì)點(diǎn)在平面電場(chǎng)中運(yùn)動(dòng)簡(jiǎn)圖Fig.1 Motion schematic diagram of a charged particle in a plane with electric field

其中，θ 為運(yùn)動(dòng)曲線的切向量與x軸的夾角，可以理解為質(zhì)點(diǎn)運(yùn)動(dòng)的偏航轉(zhuǎn)速.事實(shí)上，質(zhì)點(diǎn)在光滑曲線槽中運(yùn)動(dòng)時(shí)，受到槽對(duì)它的約束作用，使其速度滿足約束方程(2)，因此質(zhì)點(diǎn)受到槽對(duì)它的非完整約束力.但如果沒(méi)有軌道槽，質(zhì)點(diǎn)在平面上做光滑曲線運(yùn)動(dòng)時(shí)，其運(yùn)動(dòng)速度也必須滿足方程(2).若某時(shí)刻質(zhì)點(diǎn)突然有一個(gè)不為零的橫向速度，就會(huì)導(dǎo)致質(zhì)點(diǎn)的運(yùn)動(dòng)軌跡出現(xiàn)一個(gè)尖點(diǎn)，不是光滑曲線.

假設(shè)質(zhì)點(diǎn)在平面運(yùn)動(dòng)時(shí)坐標(biāo)表示為光滑曲線r=(x(t);y(t))，令該運(yùn)動(dòng)軌跡曲線切向量長(zhǎng)度為

結(jié)合式(2)和式(3)，可以得到

方程(4)表明了質(zhì)點(diǎn)沿光滑軌跡曲線運(yùn)動(dòng)的主要特征：沿與切向垂直的橫向速度為零，軌跡曲線的切向量就是質(zhì)點(diǎn)沿該曲線運(yùn)動(dòng)的前向速度.

方程(4)其實(shí)可以等價(jià)于

方程(2)是質(zhì)點(diǎn)運(yùn)動(dòng)軌跡是光滑曲線的必要條件.當(dāng)質(zhì)點(diǎn)運(yùn)動(dòng)滿足方程(4)時(shí)，可以推出其前向速度和偏航轉(zhuǎn)速與實(shí)際運(yùn)動(dòng)軌跡曲線之間的關(guān)系.

1.2 質(zhì)點(diǎn)的運(yùn)動(dòng)速度與實(shí)際運(yùn)動(dòng)軌跡曲線的關(guān)系

對(duì)式(6)關(guān)于t求導(dǎo)得

將式(6)和(7)交叉相乘并相減得

因此，質(zhì)點(diǎn)運(yùn)動(dòng)速度與運(yùn)動(dòng)軌跡曲線之間的關(guān)系為

從式(9)可以看出，給定一個(gè)運(yùn)動(dòng)軌跡曲線可以確定質(zhì)點(diǎn)運(yùn)動(dòng)的前向速度和偏航轉(zhuǎn)速；反之，根據(jù)式(6)，給定質(zhì)點(diǎn)運(yùn)動(dòng)的前向速度和偏航轉(zhuǎn)速，可以確定質(zhì)點(diǎn)運(yùn)動(dòng)軌跡的參數(shù)方程.因此，利用式(9)，可以將參數(shù)曲線表示成速度形式.

事實(shí)上，對(duì)于一般的機(jī)械力學(xué)系統(tǒng)，利用Euler-Lagrange 方程建模得到的動(dòng)力學(xué)方程基本上都是關(guān)于速度的，所以將目標(biāo)軌跡曲線轉(zhuǎn)化為速度目標(biāo)后，能非常方便地設(shè)計(jì)力或力矩控制器.

2 獨(dú)輪的約束方程與動(dòng)力學(xué)方程

如圖2 所示，假設(shè)獨(dú)輪足夠?qū)捛覠o(wú)側(cè)翻和側(cè)滑，則獨(dú)輪的中心O在平面上做連續(xù)光滑運(yùn)動(dòng).假設(shè)該中心坐標(biāo)為(x(t);y(t))，故其運(yùn)動(dòng)需滿足方程(2).在這個(gè)實(shí)際問(wèn)題中，要使得中心O的運(yùn)動(dòng)滿足方程(2)，整個(gè)力學(xué)系統(tǒng)要受到非完整約束力的作用才能實(shí)現(xiàn).因此，約束方程(2)是獨(dú)輪運(yùn)動(dòng)受到的非完整約束.考慮到獨(dú)輪的運(yùn)動(dòng)是通過(guò)驅(qū)動(dòng)輪子轉(zhuǎn)動(dòng)來(lái)實(shí)現(xiàn)的，所以必須引入輪子轉(zhuǎn)速這個(gè)變量.假設(shè)輪子不打滑和不空轉(zhuǎn)，則獨(dú)輪還受到如下完整約束

其中，r是獨(dú)輪半徑，是獨(dú)輪轉(zhuǎn)速.

圖2 獨(dú)輪運(yùn)動(dòng)結(jié)構(gòu)簡(jiǎn)圖Fig.2 Motion schematic diagram of the single-wheel

因此，獨(dú)輪總共受到兩個(gè)約束作用，式(10)是一個(gè)完整約束，而式(2)是獨(dú)輪受到的非完整約束.

獨(dú)輪運(yùn)動(dòng)的動(dòng)力學(xué)方程為

要使中心點(diǎn)O沿給定軌跡曲線運(yùn)動(dòng)，該如何設(shè)計(jì)控制力矩呢?由于中心O的運(yùn)動(dòng)滿足方程(2)，由式(9)可將目標(biāo)曲線轉(zhuǎn)化為速度目標(biāo)，將式(10)代入式(11)，動(dòng)力學(xué)方程轉(zhuǎn)化為

這樣就將原問(wèn)題轉(zhuǎn)化為線性系統(tǒng)(12)關(guān)于速度目標(biāo)(9)的一般軌跡跟蹤控制問(wèn)題.

3 兩輪式倒立擺的約束方程與動(dòng)力學(xué)方程

兩輪式倒立擺是一類經(jīng)典欠驅(qū)動(dòng)的非完整系統(tǒng)，在實(shí)際中應(yīng)用廣泛，相應(yīng)的運(yùn)動(dòng)規(guī)劃和控制研究非常多[1;22].如圖3 所示，假設(shè)兩輪無(wú)側(cè)滑無(wú)空轉(zhuǎn)，左右兩輪各安裝一個(gè)驅(qū)動(dòng)馬達(dá)，通過(guò)驅(qū)動(dòng)左右兩輪轉(zhuǎn)動(dòng)來(lái)實(shí)現(xiàn)兩輪式倒立擺的運(yùn)動(dòng)和擺桿平衡.

圖3 兩輪式倒立擺結(jié)構(gòu)簡(jiǎn)圖Fig.3 Schematic diagram of the two-wheeled inverted pendulum

3.1 兩輪運(yùn)動(dòng)的約束方程

設(shè)中心O坐標(biāo)為(x(t);y(t))，當(dāng)兩輪式倒立擺在平面上無(wú)側(cè)滑時(shí)，O在平面上沿光滑曲線運(yùn)動(dòng)，故其運(yùn)動(dòng)必須滿足約束方程(2).另外，當(dāng)兩輪不打滑不空轉(zhuǎn)時(shí)，有如下速度關(guān)系式

因此，兩輪移動(dòng)結(jié)構(gòu)在運(yùn)動(dòng)時(shí)總共受到一個(gè)非完整約束(2)和兩個(gè)完整約束(13).從方程(13)中將反解出來(lái)，并結(jié)合方程(5)，則可將兩輪式移動(dòng)結(jié)構(gòu)運(yùn)動(dòng)時(shí)所受約束方程總結(jié)為

實(shí)際上兩輪式移動(dòng)結(jié)構(gòu)只受到3 個(gè)約束作用，式(14)中其實(shí)包含了一個(gè)賦值關(guān)系式(3).

3.2 兩輪式倒立擺的動(dòng)力學(xué)方程

兩輪式倒立擺的擺桿穩(wěn)定是通過(guò)整個(gè)結(jié)構(gòu)前后運(yùn)動(dòng)的耦合作用來(lái)實(shí)現(xiàn)的，這里面受力分析比較復(fù)雜.目前求解其動(dòng)力學(xué)方程的方法主要有Newton 法[23]、Kane 法[24]和Euler-Lagrange 方法.相對(duì)來(lái)說(shuō)，Newton 法和Kane 法都比較繁瑣.這里我們采用Euler-Lagrange 方程來(lái)建立其動(dòng)力學(xué)方程[22].

將式(4)的第二式代入式(14)的第三、四式，并結(jié)合式(2)，得到兩輪結(jié)構(gòu)運(yùn)動(dòng)約束的另一種形式

令q=是兩輪式倒立擺的廣義坐標(biāo)，則運(yùn)動(dòng)約束方程(15)可以寫(xiě)成矩陣形式，其中

另一方面，令V=，約束(14)寫(xiě)成矩陣形式

采用Euler-Lagrange 方程來(lái)求解兩輪式倒立擺的動(dòng)力學(xué)方程[22].假設(shè)和M分別是輪子和中間體質(zhì)量，分別是輪子繞輪軸方向和z軸方向的轉(zhuǎn)動(dòng)慣量，IB和Iz分別是中間體繞輪軸方向和z軸方向的轉(zhuǎn)動(dòng)慣量.

兩輪的前向動(dòng)能和轉(zhuǎn)動(dòng)動(dòng)能之和為

中間體的前向動(dòng)能和轉(zhuǎn)動(dòng)動(dòng)能分別為

轉(zhuǎn)動(dòng)慣量Iz是和擺角相關(guān)的，在擺角較小的情況下，Iz近似一個(gè)常量.兩輪式倒立擺重力勢(shì)能為

因此，Lagrange 函數(shù)為

利用非完整力學(xué)系統(tǒng)的Euler-Lagrange 方程

其中，λ 是Lagrange 乘子，T是輸入力矩向量，E(q)是輸入匹配矩陣

計(jì)算式(18)并按照狀態(tài)變量的各階導(dǎo)數(shù)整理成為了消去式(19)中的Lagrange 乘子，將式(16)的左右兩端分別對(duì)t求導(dǎo)并代入式(19)得

對(duì)式(20)左右兩邊同時(shí)左乘ST(q)，并利用式(17)將Lagrange 乘子消去得

將式(22)展開(kāi)，并改寫(xiě)成狀態(tài)方程的形式

其中

3.3 兩輪式倒立擺的運(yùn)動(dòng)控制

要使兩輪式倒立擺的中心點(diǎn)O沿給定的軌跡曲線運(yùn)動(dòng)，并且整個(gè)運(yùn)動(dòng)過(guò)程中擺角始終保持足夠小，該如何設(shè)計(jì)控制器(u1;u2)呢?利用式(9)將目標(biāo)軌跡曲線轉(zhuǎn)化為速度目標(biāo)的形式，結(jié)合式(23)的后兩式可將原問(wèn)題轉(zhuǎn)化為一般的軌跡跟蹤控制問(wèn)題.另外，動(dòng)力學(xué)方程(23)可以解耦成兩個(gè)控制子系統(tǒng)，可以分別設(shè)計(jì)控制器u2和u1.在文獻(xiàn)[25]中，通過(guò)引入關(guān)于擺角的大權(quán)重性能指標(biāo)，對(duì)擺角進(jìn)行線性化后將得到的線性系統(tǒng)看成標(biāo)稱系統(tǒng)，然后設(shè)計(jì)關(guān)于性能指標(biāo)的最優(yōu)積分滑?？刂破饕詫?shí)現(xiàn)兩輪式倒立擺的往返運(yùn)動(dòng).

4 三四輪移動(dòng)結(jié)構(gòu)的約束方程與動(dòng)力學(xué)方程

4.1 三四輪移動(dòng)結(jié)構(gòu)的運(yùn)動(dòng)約束

如圖4 和圖5 所示三輪和四輪移動(dòng)結(jié)構(gòu)，假設(shè)驅(qū)動(dòng)馬達(dá)都安裝在后兩輪上，前輪可以通過(guò)控制方向盤(pán)進(jìn)行轉(zhuǎn)向.后兩輪所受到的約束和兩輪式倒立擺是一樣的，都可以用式(14)表示.

對(duì)于前輪所受約束，考慮中間桿的前端點(diǎn)P(xP;yP)的運(yùn)動(dòng).注意到P點(diǎn)有兩重身份，首先作為前輪的中心點(diǎn)，它的運(yùn)動(dòng)滿足橫向速度為零

另一方面，P作為中間桿前端點(diǎn)，它的運(yùn)動(dòng)速度如圖6 所示，結(jié)合式(24)有如下關(guān)系

圖4 三輪移動(dòng)結(jié)構(gòu)簡(jiǎn)圖Fig.4 Schematic diagram of the three-wheeled mobile structure

圖5 四輪移動(dòng)結(jié)構(gòu)簡(jiǎn)圖Fig.5 Schematic diagram of the four-wheeled mobile structure

圖6 P 點(diǎn)速度示意圖Fig.6 Schematic diagram of the velocity of P

將式(25)改寫(xiě)為

方程(26)是一個(gè)非完整約束，它刻畫(huà)了前輪轉(zhuǎn)向角與中心點(diǎn)O的速度之間的關(guān)系.若將=vcos θ 代入式(26)，還可以將該非完整約束改寫(xiě)為

4.2 三輪移動(dòng)結(jié)構(gòu)的動(dòng)力學(xué)方程

由于圖4 和圖5 中三、四輪結(jié)構(gòu)所受的約束方程完全相同，因此利用方程(18)求解動(dòng)力學(xué)方程的過(guò)程完全一樣.僅以三輪為例，是通過(guò)方向盤(pán)人為進(jìn)行控制的，所以將看成控制變量.結(jié)合式(14)和式(26)，三輪移動(dòng)結(jié)構(gòu)所受到的所有運(yùn)動(dòng)約束為

令q=(x;y;θ;θl;θr)T,V=，將約束方程(27)寫(xiě)成矩陣形式

從式(27)來(lái)看，三輪移動(dòng)結(jié)構(gòu)似乎一共受到四個(gè)約束作用，但其實(shí)不然，約束方程(26)和(13)的第二式并不是相互獨(dú)立的，而是互相影響的.在兩輪式移動(dòng)結(jié)構(gòu)的情形中，兩輪式的三個(gè)約束分別是橫向速度為零，兩輪轉(zhuǎn)速之和決定前向速度，兩輪轉(zhuǎn)速之差決定偏航轉(zhuǎn)速.而在本文的三輪移動(dòng)結(jié)構(gòu)中，3 個(gè)約束作用實(shí)際上分別是橫向速度為零，兩輪轉(zhuǎn)速之和決定前向速度，前向速度v和決定偏航轉(zhuǎn)速，此時(shí)對(duì)應(yīng)的就是確定的了.因此，我們將這兩個(gè)約束整合在一起，將三輪移動(dòng)結(jié)構(gòu)受到的約束作用總結(jié)成如下形式

將其寫(xiě)成矩陣形式

注意到，F(xiàn)(q);S(q)滿足如下關(guān)系

為了利用Euler-Lagrange 方程來(lái)計(jì)算三輪移動(dòng)結(jié)構(gòu)的動(dòng)力學(xué)方程，首先需要分別計(jì)算各個(gè)部分的動(dòng)能.如圖6 所示，可以得到如下位置關(guān)系

因此，兩后輪的前向動(dòng)能和轉(zhuǎn)動(dòng)動(dòng)能之和為

中間桿和前輪的前向動(dòng)能和轉(zhuǎn)動(dòng)動(dòng)能之和分別為

其中，MB是中間桿的質(zhì)量，IB是中間桿繞O點(diǎn)在x?y平面上的轉(zhuǎn)動(dòng)慣量.

將Lagrange 函數(shù)L=+TB+Tf代入式(18)，此時(shí)輸入力矩向量T和匹配矩陣E(q)分別為

計(jì)算式(18)，并按狀態(tài)變量的各階導(dǎo)數(shù)進(jìn)行整理得

對(duì)式(28)關(guān)于t求導(dǎo)，然后代入式(30)得

對(duì)式(31)左右兩邊同時(shí)左乘ST(q)，并利用式(29)將Lagrange 乘子消去得三輪移動(dòng)結(jié)構(gòu)動(dòng)力學(xué)方程為

4.3 三輪移動(dòng)結(jié)構(gòu)的運(yùn)動(dòng)控制

動(dòng)力學(xué)方程(32)可以寫(xiě)成狀態(tài)方程形式

利用式(9)將目標(biāo)軌跡曲線轉(zhuǎn)化成速度形式，結(jié)合控制系統(tǒng)(33)得到一般的軌跡跟蹤控制問(wèn)題.注意這里u1是中間控制變量，而最終需要設(shè)計(jì)的控制變量是和u2.由于方程(33)是解耦的，因此可先根據(jù)第二個(gè)方程設(shè)計(jì)u1，然后結(jié)合方程(26)得

方程(34)給出了控制變量u1和的關(guān)系.將得到的控制變量代入式(33)的第一式，再設(shè)計(jì)控制變量u2完成最終的軌跡跟蹤控制設(shè)計(jì)任務(wù).

5 動(dòng)態(tài)跟蹤目標(biāo)與運(yùn)動(dòng)控制設(shè)計(jì)

前面幾節(jié)內(nèi)容已經(jīng)系統(tǒng)地分析了各類輪式移動(dòng)結(jié)構(gòu)的約束方程及其動(dòng)力學(xué)方程.實(shí)際上約束方程(2)是所有輪式移動(dòng)結(jié)構(gòu)在無(wú)橫向滑動(dòng)時(shí)必須滿足的非完整約束.通過(guò)分析該約束條件，我們可以利用式(9)將輪式移動(dòng)結(jié)構(gòu)的任務(wù)軌跡曲線轉(zhuǎn)化為成速度目標(biāo)的形式，然后結(jié)合其動(dòng)力學(xué)方程將原運(yùn)動(dòng)任務(wù)轉(zhuǎn)化為一般的軌跡跟蹤控制問(wèn)題.如文獻(xiàn)[26]在考慮輪式倒立擺的避障運(yùn)動(dòng)控制時(shí)，就將設(shè)計(jì)好的分段多項(xiàng)式軌跡曲線轉(zhuǎn)化成近似的速度目標(biāo)形式，然后結(jié)合動(dòng)力學(xué)方程設(shè)計(jì)軌跡跟蹤控制器.文獻(xiàn)[27-29]在考慮拖--掛車的軌跡曲線跟蹤和避障運(yùn)動(dòng)時(shí)，就是先將目標(biāo)軌跡曲線轉(zhuǎn)化為式(9)這樣的速度目標(biāo)，然后采用模型預(yù)測(cè)等控制方法實(shí)現(xiàn)給定的運(yùn)動(dòng)任務(wù).但從這些論文的仿真結(jié)果來(lái)看，不管采用多么先進(jìn)的控制設(shè)計(jì)方法，實(shí)際運(yùn)動(dòng)軌跡總是會(huì)有一些偏離目標(biāo)軌跡曲線，尤其是在最初始的一段時(shí)間內(nèi).出現(xiàn)這樣的結(jié)果主要原因之一在于所采用的速度目標(biāo)(9)在初始時(shí)刻和實(shí)際速度相比具有比較大的差值，這個(gè)速度誤差會(huì)累計(jì)成很大的位置誤差.另外，如果采用靜態(tài)速度目標(biāo)(9)，當(dāng)前向速度誤差控制系統(tǒng)受到未知擾動(dòng)影響時(shí)，會(huì)導(dǎo)致實(shí)際前向速度偏離給定的前向速度目標(biāo)，而此時(shí)偏航轉(zhuǎn)速目標(biāo)不能時(shí)時(shí)地進(jìn)行調(diào)整，這就會(huì)導(dǎo)致輪式移動(dòng)結(jié)構(gòu)偏離目標(biāo)軌跡曲線.為了同時(shí)解決這兩個(gè)問(wèn)題，使輪式移動(dòng)結(jié)構(gòu)能精確地沿著給定目標(biāo)軌跡曲線運(yùn)動(dòng)，我們將靜態(tài)速度目標(biāo)(9)改進(jìn)為動(dòng)態(tài)跟蹤目標(biāo).

5.1 動(dòng)態(tài)跟蹤目標(biāo)設(shè)計(jì)

其中k(s(t))=是目標(biāo)曲線的相對(duì)曲率.

事實(shí)上，曲率函數(shù)k(s(t))是曲線的核心特征.當(dāng)參數(shù)t=s是弧長(zhǎng)參數(shù)時(shí)，前向速度目標(biāo)是單位速度，此時(shí)曲率函數(shù)k(s)在平面上唯一確定一條光滑曲線.當(dāng)t不是弧長(zhǎng)參數(shù)時(shí)，由前向速度和曲率函數(shù)k(t)=k(s(t))可以在平面上唯一確定一條光滑曲線.對(duì)曲線引入新的時(shí)間變量η,它由t=φ(η)確定，其中φ 是一一對(duì)應(yīng)的光滑函數(shù)，則可將原參數(shù)曲線轉(zhuǎn)化為=.由復(fù)合函數(shù)求導(dǎo)關(guān)系

從上式可看出，方程(35)中的前向速度目標(biāo)是可以通過(guò)參數(shù)變換進(jìn)行調(diào)整的.因此方程(35)中涉及到曲線曲率的第二式才是軌跡曲線的本質(zhì)刻畫(huà).

在實(shí)際跟蹤過(guò)程中，如果我們只需要輪式移動(dòng)結(jié)構(gòu)精確沿給定軌跡曲線運(yùn)動(dòng)，而并不在乎整個(gè)過(guò)程中運(yùn)動(dòng)速度的快慢，那么可以放棄對(duì)前向速度目標(biāo)的精確跟蹤，集中精力跟蹤方程(35)的第二式.此時(shí)可以將跟蹤目標(biāo)(35)改進(jìn)為動(dòng)態(tài)跟蹤目標(biāo)

一般來(lái)說(shuō)，φ(t)可設(shè)計(jì)為

這里l是目標(biāo)軌跡曲線的長(zhǎng)度，此時(shí)

在實(shí)際應(yīng)用中，由于初始時(shí)刻輪式移動(dòng)結(jié)構(gòu)都是靜止的，利用前向速度目標(biāo)(37)可以使得跟蹤控制問(wèn)題的初始速度誤差為零，并可以根據(jù)實(shí)際需要調(diào)節(jié)參數(shù)β 使得整個(gè)過(guò)程跟蹤控制的效果最好.

5.2 動(dòng)態(tài)跟蹤目標(biāo)的優(yōu)點(diǎn)

與靜態(tài)速度目標(biāo)(9)相比，采用動(dòng)態(tài)跟蹤目標(biāo)(36)具有兩個(gè)明顯的優(yōu)點(diǎn)：

(1)速度誤差(尤其初始速度誤差)不會(huì)累積成越來(lái)越大的位置誤差.由于動(dòng)態(tài)跟蹤目標(biāo)直接指向目標(biāo)曲線的本質(zhì)-曲率，當(dāng)實(shí)際前向速度偏大時(shí)，方程(36)第二式所給的偏航轉(zhuǎn)速目標(biāo)也變得更大，只要控制過(guò)程中的偏航轉(zhuǎn)速誤差足夠小，也即曲率跟蹤誤差足夠小，就能保證輪式移動(dòng)結(jié)構(gòu)不偏離給定軌道向外偏轉(zhuǎn).反之，若實(shí)際前向速度偏小，那么方程(36)第二式所給出的偏航轉(zhuǎn)速目標(biāo)也會(huì)相應(yīng)變小，只要曲率跟蹤誤差小就能保證輪式移動(dòng)結(jié)構(gòu)不偏離給定軌道向內(nèi)偏轉(zhuǎn).因此，盡管在整個(gè)運(yùn)動(dòng)過(guò)程中存在很大的前向速度誤差，但采用動(dòng)態(tài)跟蹤目標(biāo)能夠保證輪式移動(dòng)結(jié)構(gòu)始終在給定軌道上運(yùn)動(dòng).

(2)前向速度目標(biāo)可以根據(jù)實(shí)際需要進(jìn)行設(shè)計(jì)，這在實(shí)際應(yīng)用中非常重要.由于對(duì)前向速度目標(biāo)無(wú)限制要求，而采用不同的前向速度目標(biāo)，所導(dǎo)致的結(jié)果僅僅是整個(gè)運(yùn)動(dòng)過(guò)程中輪式移動(dòng)結(jié)構(gòu)運(yùn)動(dòng)快慢不同而已.另外，考慮到輪胎最大靜摩擦力是有限的，因此實(shí)際控制力矩最好不要超過(guò)該最大靜摩擦力.這里我們可以通過(guò)設(shè)計(jì)小一些的前向速度目標(biāo)，使得實(shí)際需要的控制力矩盡量小于輪胎的最大靜摩擦力，以免輪胎打滑嚴(yán)重影響跟蹤控制效果.

5.3 基于動(dòng)態(tài)跟蹤目標(biāo)的控制仿真

以單位圓為目標(biāo)軌跡曲線為例，由于目標(biāo)曲線的相對(duì)曲率是常數(shù)1，文獻(xiàn)[30]中將偏航轉(zhuǎn)速目標(biāo)設(shè)計(jì)為實(shí)際的前向速度，這樣設(shè)計(jì)的動(dòng)態(tài)跟蹤目標(biāo)恰好是式(36)的特殊情形.由于兩輪式倒立擺的動(dòng)力學(xué)方程是非線性的，原問(wèn)題轉(zhuǎn)化為非線性系統(tǒng)的軌跡跟蹤控制問(wèn)題.為此，我們先利用反饋線性化方法將系統(tǒng)轉(zhuǎn)化為線性系統(tǒng)，然后基于線性系統(tǒng)利用預(yù)測(cè)反饋處理輸入時(shí)滯的影響.而線性化誤差和外部有界擾動(dòng)整體打包后利用積分滑?？刂苼?lái)處理.由于所設(shè)計(jì)的積分滑模面就是兩輪式倒立擺需要完成的運(yùn)動(dòng)任務(wù)對(duì)應(yīng)的狀態(tài)，因此最終設(shè)計(jì)的積分滑模控制不但能使兩輪式倒立擺在保持?jǐn)[桿穩(wěn)定下很好地完成給定的運(yùn)動(dòng)任務(wù)，整個(gè)運(yùn)動(dòng)過(guò)程還具有很強(qiáng)的魯棒性.如圖7 所示，實(shí)際運(yùn)動(dòng)軌跡幾乎和單位圓是重合的，而采用靜態(tài)速度目標(biāo)(9)時(shí)，實(shí)際運(yùn)動(dòng)軌跡和目標(biāo)曲線會(huì)有很大的偏差.

圖7 兩輪式倒立擺在平面上實(shí)際運(yùn)動(dòng)軌跡Fig.7 Actual motion trajectory of the two-wheeled inverted pendulum in a plane

文獻(xiàn)[30]中給定的目標(biāo)軌跡曲線是單位圓，是具有常數(shù)曲率的特殊曲線.為了更好地說(shuō)明采用動(dòng)態(tài)跟蹤目標(biāo)的控制效果，我們考慮獨(dú)輪沿一個(gè)非常數(shù)曲率的光滑曲線運(yùn)動(dòng).假設(shè)初始時(shí)刻獨(dú)輪是靜止的，需要完成的運(yùn)動(dòng)任務(wù)軌跡曲線是擺線

由曲線基本理論可計(jì)算出該軌跡曲線的曲率為

注意到該曲線在端點(diǎn)處的曲率趨于無(wú)窮，在應(yīng)用動(dòng)態(tài)跟蹤目標(biāo)(36)進(jìn)行控制時(shí)，會(huì)出現(xiàn)極大的數(shù)值計(jì)算誤差.因此我們選取擺線中不包含上述端點(diǎn)的一段曲線作為目標(biāo)軌跡曲線

此時(shí)，上述目標(biāo)軌跡曲線的曲率可以表示為

另外，曲線的弧長(zhǎng)函數(shù)為

根據(jù)式(36)設(shè)計(jì)動(dòng)態(tài)跟蹤目標(biāo)

令X=[x1;x2]T=[s;v]T，前向速度控制方程改寫(xiě)為二階狀態(tài)方程

再將控制方程(42)轉(zhuǎn)化成誤差系統(tǒng)，令

則前向速度誤差系統(tǒng)為

要使得整個(gè)運(yùn)動(dòng)過(guò)程中前向速度誤差一直保持足夠小，可以采用文獻(xiàn)[22]中的思想，利用線性二次型最優(yōu)控制的權(quán)重來(lái)調(diào)節(jié).由于式(40)中前向速度目標(biāo)?v是逐漸趨近于零的，且其無(wú)窮積分是收斂.因此可以引入一個(gè)無(wú)限時(shí)域線性二次型性能指標(biāo)

該性能指標(biāo)中關(guān)于前向速度誤差的權(quán)重取足夠大，這樣所設(shè)計(jì)的最優(yōu)控制能使前向速度誤差盡量小.利用線性二次型最優(yōu)控制理論，u1設(shè)計(jì)為

針對(duì)偏航轉(zhuǎn)速控制系統(tǒng)，可以采用文獻(xiàn)[25]中的積分滑模思想設(shè)計(jì)積分滑?？刂破?令，則系統(tǒng)(41)的第二式可轉(zhuǎn)化為誤差控制系統(tǒng)

其中k是控制增益參數(shù)(后面所有仿真中k=40).然后將動(dòng)態(tài)跟蹤目標(biāo)設(shè)計(jì)為積分滑模面，其中G>0 是合適的常數(shù)

這樣，切換控制設(shè)計(jì)為

其中μ是滑模控制參數(shù)，D是外部擾動(dòng)的最大振幅.最終u2設(shè)計(jì)為

這樣設(shè)計(jì)的積分滑?？刂?45)具有很強(qiáng)的魯棒性.

若采用靜態(tài)速度目標(biāo)(9)的形式，即

若同樣采用式(44)和式(45)作為控制器，由于?v不是逐漸趨于零的，從文獻(xiàn)[22]的分析可知，此時(shí)不能采用無(wú)限時(shí)域的性能指標(biāo)，而必須采用有限時(shí)域性能指標(biāo)，導(dǎo)致對(duì)應(yīng)的黎卡提微分方程很難求解，控制器設(shè)計(jì)變得困難.而采用動(dòng)態(tài)目標(biāo)(40)時(shí)，只需求解代數(shù)黎卡提方程，控制器設(shè)計(jì)變得簡(jiǎn)單.

一般來(lái)說(shuō)，采用靜態(tài)目標(biāo)(46)時(shí)，會(huì)使得誤差系統(tǒng)的初始值不為零.而采用動(dòng)態(tài)目標(biāo)(40)時(shí)，可以根據(jù)實(shí)際需要設(shè)計(jì)合適的前向速度目標(biāo)，使得初始速度誤差為零.如圖8 和圖9 所示，明顯采用靜態(tài)跟蹤目標(biāo)時(shí)的速度累積誤差會(huì)遠(yuǎn)遠(yuǎn)大于采用動(dòng)態(tài)跟蹤目標(biāo)時(shí).而累積速度誤差大自然會(huì)導(dǎo)致累積位置誤差也很大，使最終得到的實(shí)際軌跡曲線嚴(yán)重偏離目標(biāo)曲線.如圖10 所示，采用動(dòng)態(tài)跟蹤目標(biāo)(40)時(shí)，實(shí)際軌跡偏差非常小，而采用靜態(tài)跟蹤目標(biāo)(46)時(shí)，實(shí)際軌跡嚴(yán)重偏離目標(biāo)曲線.另外，圖8 和圖9 中紅色的線在大概一半的時(shí)間時(shí)就結(jié)束了，這是因?yàn)椴捎渺o態(tài)目標(biāo)(46)時(shí)的實(shí)際平均速度要比采用動(dòng)態(tài)目標(biāo)(40)時(shí)更快，提前完成了運(yùn)動(dòng)任務(wù).

圖8 獨(dú)輪的前向速度誤差Fig.8 Forward speed error of the single-wheel

圖9 獨(dú)輪的偏航轉(zhuǎn)速誤差Fig.9 Yaw rotation speed error of the single-wheel

圖10 獨(dú)輪在平面上實(shí)際運(yùn)動(dòng)軌跡Fig.10 Actual motion trajectory of the single-wheel

實(shí)事上，在采用動(dòng)態(tài)跟蹤目標(biāo)(40)時(shí)，由于偏航轉(zhuǎn)速目標(biāo)是根據(jù)實(shí)際前向速度時(shí)時(shí)調(diào)整的，能夠極大地減少累積位置誤差.即使前向速度誤差非常大甚至誤差系統(tǒng)不穩(wěn)定，只要偏航轉(zhuǎn)速目標(biāo)能被精確跟蹤，就能保證獨(dú)輪沿著給定的目標(biāo)軌跡曲線運(yùn)動(dòng).考慮誤差系統(tǒng)(43)受到未知擾動(dòng)d(t)影響的情形

當(dāng)d(t)=[5y1;10:5y2]T時(shí)，若在d(t)的影響下繼續(xù)采用控制器(44)和(45)，則此時(shí)誤差系統(tǒng)(47)會(huì)變得不穩(wěn)定，其前向速度誤差如圖11 所示.但從圖12 可以看出，雖然前向速度誤差很大，但由于采用了動(dòng)態(tài)跟蹤目標(biāo)(40)，其實(shí)際運(yùn)動(dòng)軌跡依然和目標(biāo)曲線非常接近，實(shí)現(xiàn)了任務(wù)目標(biāo)精確跟蹤的目的.

圖11 d(t)影響下獨(dú)輪的前向速度誤差Fig.11 Forward speed error of the single-wheel under the effect of d(t)

圖12 d(t)影響下獨(dú)輪在平面上實(shí)際運(yùn)動(dòng)軌跡Fig.12 Actual motion trajectory of the single-wheel under the effect of d(t)

6 結(jié)論

不同類型輪式移動(dòng)結(jié)構(gòu)在平面上不發(fā)生橫向滑動(dòng)時(shí)都會(huì)受到一個(gè)共同的非完整約束，該非完整約束本質(zhì)上就是質(zhì)點(diǎn)沿平面光滑曲線運(yùn)動(dòng)時(shí)沿與切向垂直的橫向方向的速度為零.本文理清了各輪式移動(dòng)結(jié)構(gòu)的所有約束方程，在此基礎(chǔ)上，給出了基于Euler-Lagrange 方程建立不同輪式移動(dòng)結(jié)構(gòu)動(dòng)力學(xué)方程的一般方法.另外，基于該非完整約束，我們可以將目標(biāo)軌跡曲線轉(zhuǎn)化為速度目標(biāo)的形式，然后引入相對(duì)曲率設(shè)計(jì)動(dòng)態(tài)跟蹤目標(biāo).仿真結(jié)果顯示，即使前向速度跟蹤誤差非常大，甚至前向速度誤差系統(tǒng)不穩(wěn)定，采用動(dòng)態(tài)跟蹤目標(biāo)都能保證輪式移動(dòng)結(jié)構(gòu)不偏離給定目標(biāo)軌跡曲線.還可以通過(guò)設(shè)計(jì)合適的前向速度目標(biāo)使得跟蹤的效果達(dá)到最好.

本文所提出的動(dòng)態(tài)跟蹤目標(biāo)方法從根本上解決了軌跡曲線精確跟蹤的問(wèn)題，其他任何先進(jìn)控制設(shè)計(jì)方法僅僅從控制設(shè)計(jì)的角度是達(dá)不到這樣的精確效果的.實(shí)事上，受到該非完整約束的各類型機(jī)械結(jié)構(gòu)的精確運(yùn)動(dòng)控制問(wèn)題都可以采用動(dòng)態(tài)跟蹤目標(biāo)方法.甚至只要是需要精確跟蹤給定光滑軌跡曲線的控制問(wèn)題，都可以采用該方法，達(dá)到精確跟蹤的目的.進(jìn)一步地，后續(xù)工作還可以引入撓率，將動(dòng)態(tài)跟蹤目標(biāo)方法推廣到空間軌跡曲線的情形，這將大大擴(kuò)展動(dòng)態(tài)跟蹤目標(biāo)思想的應(yīng)用范圍.

致謝衷心地感謝審稿人認(rèn)真審閱我們的論文，給出的寶貴意見(jiàn)使論文得到極大的改進(jìn).