李 亮 劉一方 譚躍剛

1.寶雞文理學院機械工程學院,寶雞,7210162.中國長江動力集團有限公司,武漢,4300703.武漢理工大學機電工程學院，武漢，430070

一類非完整機械手的鏈式變換特性及其運動規(guī)劃方法

李 亮1劉一方2譚躍剛3

1.寶雞文理學院機械工程學院,寶雞,7210162.中國長江動力集團有限公司,武漢,4300703.武漢理工大學機電工程學院，武漢，430070

針對一類非完整三關(guān)節(jié)機械手提出了規(guī)避鏈式逆變換奇異位形的運動規(guī)劃方法。根據(jù)非完整機械手的結(jié)構(gòu)特點分離出廣義坐標向量中的自由變量，將其邊界值作為控制輸入中的待定系數(shù)進行求解。對于非完整機械手在鏈式逆變換過程中出現(xiàn)的奇異性問題，提出令鏈式空間中一條軌跡向關(guān)節(jié)空間映射有解的不等式約束條件，利用增加附加位形約束的方法來規(guī)避非完整機械手的鏈式逆變換奇異位形。仿真與實驗結(jié)果證明了運動規(guī)劃算法的可行性與有效性。

非完整機械手； 鏈式變換； 奇異位形；運動規(guī)劃

0 引言

機器人運動學方程中，對時間的導(dǎo)數(shù)不能積分為有限形式，這類約束稱為非完整約束，含有非完整約束的系統(tǒng)稱為非完整系統(tǒng)。雖然非完整約束限制了系統(tǒng)的瞬時運動，但在位形空間上是完全可控的，因此非完整系統(tǒng)具有更高的靈活性。此外，非完整系統(tǒng)的廣義速度并不是相互獨立的，故其控制輸入數(shù)目少于廣義坐標數(shù)目，呈現(xiàn)出欠驅(qū)動的特性，驅(qū)動裝置的減少使得非完整系統(tǒng)具有結(jié)構(gòu)緊湊、重量輕等特點。

非完整運動規(guī)劃即在有限時間內(nèi)使機器人系統(tǒng)從給定的初始位形運動到目標位形的控制輸入求解問題[1]。非完整系統(tǒng)內(nèi)部存在運動耦合，因此在機器人系統(tǒng)的位形空間內(nèi)只有滿足非完整約束的軌跡才是可行的。這一特性使得針對完整系統(tǒng)運動規(guī)劃的純幾何方法都無法直接應(yīng)用于非完整系統(tǒng)，故對非完整系統(tǒng)的運動規(guī)劃顯得更為復(fù)雜和困難。非完整系統(tǒng)的運動規(guī)劃策略研究引起了國內(nèi)外學者的廣泛關(guān)注。

圍繞鏈式系統(tǒng)路徑規(guī)劃的研究，主要集中在控制算法的設(shè)計和已有可鏈式化非完整系統(tǒng)的運動控制。MURRAY等[2]通過坐標及輸入變換將輪式移動機器人的運動學方程轉(zhuǎn)換成為一類結(jié)構(gòu)簡單的非完整系統(tǒng)——鏈式系統(tǒng)。TILBURY等[3]將三角函數(shù)、多項式、分段常數(shù)等含有待定系數(shù)的方程作為控制輸入，實現(xiàn)了對鏈式系統(tǒng)的運動規(guī)劃。LOOCK等[4]基于微分代數(shù)法研究輪式移動機器人，提出了具有平滑輸出特征的路徑規(guī)劃方法。JANIAK等[5]利用牛頓下降法解決了能量最優(yōu)條件下的路徑規(guī)劃問題。

雖然針對鏈式系統(tǒng)已有許多較成熟的運動規(guī)劃算法，然而對非完整多關(guān)節(jié)機械手這類運動耦合度高、運動學模型復(fù)雜的非完整系統(tǒng)的運動規(guī)劃并不容易[6]。由于非完整機械手的運動學模型與鏈式模型之間的映射在位形空間上并不完全微分同胚，在鏈式空間中規(guī)劃所得的一條路徑不能保證其能夠完全映射至關(guān)節(jié)空間，即機械手某些關(guān)節(jié)在位形空間產(chǎn)生了模型變換奇異點或區(qū)間，如Nakamura非完整機械臂[7]、輪式拖掛車系統(tǒng)[8-9]等。盡管上述研究都已提到鏈式逆變換解的奇異性問題，但未深入討論解決辦法。本文以一類非完整三關(guān)節(jié)機械手為研究對象，利用懲罰函數(shù)和牛頓迭代法求解鏈式空間中一條鏈式逆變換有唯一解的軌跡。

1 非完整機械手的基本結(jié)構(gòu)

非完整機械手以摩擦圓盤機構(gòu)為運動傳遞單元，摩擦盤之間無滑動的滾動產(chǎn)生了不可積的速度約束，因此這類機械手系統(tǒng)為一階非完整系統(tǒng)[10]。摩擦圓盤運動傳遞機構(gòu)如圖1所示，其中盤A為輸入盤，盤B與盤D為輸出盤。當盤A旋轉(zhuǎn)時，通過滾動摩擦依次驅(qū)動盤B、C、D旋轉(zhuǎn)。另外，當水平布置的摩擦盤B、盤D的中心軸轉(zhuǎn)動時，各摩擦盤之間的運動傳動比會產(chǎn)生變化，因此可以通過改變水平摩擦盤中心軸的角位移來控制摩擦盤之間的傳動比。

圖1 摩擦圓盤運動傳遞機構(gòu)Fig.1 Friction disc motion transmission mechanism

在非完整機械手的每一個關(guān)節(jié)內(nèi)布置一組摩擦圓盤機構(gòu)，其中垂直布置的兩圓盤與前一個關(guān)節(jié)固連，水平布置的兩圓盤與后一個關(guān)節(jié)固連，非完整三關(guān)節(jié)機械手的機構(gòu)模型如圖2a所示,實物如圖2b所示。其中，電機2驅(qū)動關(guān)節(jié)1轉(zhuǎn)動，電機1驅(qū)動關(guān)節(jié)1的輸入盤A1轉(zhuǎn)動，摩擦圓盤機構(gòu)中的一個輸出盤B1通過齒輪傳動機構(gòu)驅(qū)動關(guān)節(jié)2中的輸入圓盤A2轉(zhuǎn)動，另一個輸出盤D1通過同步帶傳動機構(gòu)驅(qū)動關(guān)節(jié)2轉(zhuǎn)動。關(guān)節(jié)2中的運動傳遞關(guān)系與關(guān)節(jié)1相同。

(a)非完整三關(guān)節(jié)機械手機構(gòu)圖

(b)非完整三關(guān)節(jié)機械手實物圖圖2 非完整三關(guān)節(jié)機械手Fig.2 Three-joint nonholonomic manipulator

非完整三關(guān)節(jié)機械手的位形空間由四個狀態(tài)變量表示[11]：三個關(guān)節(jié)的轉(zhuǎn)動角位移θ1、θ2、θ3以及一個輸入盤的轉(zhuǎn)動角位移φ1,而控制輸入只有關(guān)節(jié)1的角速度與盤A1的角速度兩個。這說明非完整機械手系統(tǒng)可以由少于廣義坐標數(shù)目的控制輸入來控制其運動。

摩擦盤間的運動為無滑動的滾動，因此相互接觸的兩摩擦盤在接觸點處的相對速度為零。根據(jù)此運動關(guān)系可推導(dǎo)出非完整三關(guān)節(jié)機械手的運動學模型表達式：

(1)

式中，k2、k3為與結(jié)構(gòu)尺寸有關(guān)的系數(shù)；μ1、μ2分別為盤A1和關(guān)節(jié)1的角速度。

2 非完整機械手的鏈式變換特性

2.1 鏈式變換

S?rdalen在研究多掛車系統(tǒng)的鏈式變換時，將其廣義坐標向量選取為每個掛車牽引桿的轉(zhuǎn)動角位移及最末一個掛車的位置坐標，得到一種符合三角形構(gòu)造的非完整系統(tǒng)。這種三角形構(gòu)造非完整系統(tǒng)的特點是其第i個廣義速度只與i-1以后的廣義坐標(含第i-1個)及輸入有關(guān)。S?rdalen給出了符合三角形構(gòu)造的無漂移仿射系統(tǒng)轉(zhuǎn)換成為鏈式系統(tǒng)的條件及方法[8]。定理1 對于符合三角形構(gòu)造的無漂移仿射系統(tǒng)：

(2)

i∈{3,4,…,n}qi-1=(qi-1,qi,…,qn)T

對于非完整三關(guān)節(jié)機械手，取其廣義坐標向量q=(φ2,θ1,θ2,θ3)T，便可將非完整三關(guān)節(jié)機械手的運動學模型改寫為如下所示的三角形構(gòu)造形式：

(3)

式中，kg為與結(jié)構(gòu)尺寸有關(guān)的系數(shù)。

根據(jù)定理1可以證明非完整三關(guān)節(jié)機械手系統(tǒng)可通過如下坐標變換：

(4)

轉(zhuǎn)換成為兩輸入四維鏈式系統(tǒng)：

(5)

式中，v1和v2分別表示鏈式變量z1和z2的速度，由式(4)可知，v1是輸入圓盤A2的角速度，v2沒有明確的物理意義。

2.2 鏈式逆變換奇異性

通過鏈式變換與鏈式逆變換，可將鏈式系統(tǒng)的運動規(guī)劃方法應(yīng)用于非完整三關(guān)節(jié)機械手系統(tǒng)。然而，運動規(guī)劃過程中我們發(fā)現(xiàn)，在某些邊界條件約束下，雖然可以在鏈式空間內(nèi)規(guī)劃出各狀態(tài)變量的運動軌跡，但這些軌跡從鏈式空間映射到關(guān)節(jié)空間中時，機械手某些關(guān)節(jié)的運動可能產(chǎn)生奇異現(xiàn)象。

圖3 各關(guān)節(jié)角的運動軌跡Fig.3 The trajectory of each joint

此奇異現(xiàn)象是運動軌跡從鏈式空間映射到關(guān)節(jié)空間的過程中產(chǎn)生的，因此需先推導(dǎo)出鏈式逆變換表達式。鏈式變換表達式(4)表明關(guān)節(jié)角位移θ1、θ2與z2、z3變量之間的映射并不是單射，有多個關(guān)節(jié)向量θ對應(yīng)同一個向量z，因此在求反余弦函數(shù)時應(yīng)該先界定關(guān)節(jié)角θ1、θ2的運動范圍。例如，關(guān)節(jié)角位移θ1、θ2的初始值與終止值均為正值時，可計算得鏈式逆變換表達式：

(6)

觀察非完整機械手系統(tǒng)的鏈式逆變換表達式(式(6))發(fā)現(xiàn)其中存在反三角函數(shù)與二次開方運算，而這些運算對自變量有定義域的要求。如果狀態(tài)變量的取值超出了一定范圍，則會導(dǎo)致鏈式逆變換無實數(shù)解。如圖4所示，在θ1產(chǎn)生奇異的時間段(3～10 s之間)狀態(tài)變量z2的值大于零，這使得在鏈式逆變換時由開方計算得到的是復(fù)數(shù)解，從而導(dǎo)致了關(guān)節(jié)角θ1的鏈式逆變換奇異。這一實例表明，并不是鏈式空間中所有的點都可以通過鏈式逆變換映射到關(guān)節(jié)空間中。一旦在鏈式空間中規(guī)劃出一條軌跡，其上的某一點或某些點鏈式逆變換無解，則關(guān)節(jié)角的運動就會產(chǎn)生奇異，我們將非完整機械手的這種性質(zhì)稱為鏈式逆變換奇異性。

圖4 z的運動軌跡Fig.4 The trajectory of z parameter

這種由于鏈式逆變換而產(chǎn)生的奇異位形可以通過對狀態(tài)變量附加不等式約束來規(guī)避，根據(jù)數(shù)學上對反三角函數(shù)及二次開方運算中自變量的定義域要求，可以確定z2、z3需滿足的條件如下所示：

(7)

整理得

(8)

3 規(guī)避奇異位形的運動規(guī)劃方法

對非完整三關(guān)節(jié)機械手的運動規(guī)劃實際上是控制其三個關(guān)節(jié)角從給定的初始值運動到目標值，這是對機械手系統(tǒng)的邊界條件約束。另一方面，為了規(guī)避機械手系統(tǒng)的鏈式逆變換奇異位形，還需要對狀態(tài)變量的取值附加不等式約束條件，這一約束實際是限制了系統(tǒng)的空間位形，故稱之為位形約束。在鏈式空間內(nèi)規(guī)劃的軌跡必須同時滿足邊界條件約束與位形約束，才能無奇異地映射成為關(guān)節(jié)空間內(nèi)的可行軌跡。

3.1 邊界條件約束

非完整三關(guān)節(jié)機械手的邊界條件約束只限制θ1、θ2、θ3，稱其為待控制狀態(tài)變量，對φ2則沒有邊界條件約束，稱其為自由狀態(tài)變量。運動規(guī)劃的目的是在有限的時間內(nèi)求解有界控制輸入u(t)，使三個關(guān)節(jié)角分別從初始值出發(fā)，在終止時刻到達目標值。這樣，就將非完整三關(guān)節(jié)機械手廣義坐標向量中的自由變量分離出來，可將此自由變量的邊界值作為待定系數(shù)通過運動規(guī)劃求解。

(9)

其中，z2(T)、z3(T)、z4(T)表示控制輸入為u(t)時各狀態(tài)變量在終止時刻的值，是關(guān)于u(t)的函數(shù)。

3.2 位形約束

為了規(guī)避非完整三關(guān)節(jié)機械手的鏈式逆變換奇異位形，必須保證運動規(guī)劃的軌跡能夠完全地從鏈式空間映射到關(guān)節(jié)空間中，因此軌跡上的每一個點都應(yīng)該是鏈式逆變換有解的。在之前的論述中已給出了鏈式空間中的一點向關(guān)節(jié)空間映射有實數(shù)解的條件(式(8))，該式中狀態(tài)變量z2、z3相互耦合，且每增加一個不等式約束時算法的復(fù)雜度及計算時間也會大大增加。因此在實際的運動規(guī)劃中可以根據(jù)具體情況選擇其中的一個或幾個不等式來作為非完整機械手系統(tǒng)的附加位形約束。

將系統(tǒng)鏈式空間中的一條軌跡L定義為：L={z(ti):ti∈[0,T],i=1,2，…，N}，即將軌跡看作連續(xù)時間內(nèi)一系列的點，并假設(shè)空間中可達的區(qū)域由不等式x(z)<0表示。為了處理這一不等式位形約束，定義如下函數(shù)：

(10)

其中，r>0，g(x)為一個連續(xù)光滑的函數(shù)。當x小于零時，函數(shù)g(x)為零，當x大于等于零時，g(x)隨x單調(diào)遞增，選取g(x)為

(11)

γ>0

即可將不等式約束方程x(z)<0轉(zhuǎn)換為等式約束方程：

(12)

當軌跡上所有N個點均滿足位形約束條件x(z)<0時，表明軌跡L在z變量的可達空間內(nèi)，h的值為零，等式約束方程(式(12))不起作用；當存在某個或某些點不滿足位形約束條件時，h的值為正數(shù)，約束方程(式(12))迫使軌跡L向可達空間內(nèi)運動。

3.3 運動規(guī)劃

在非完整三關(guān)節(jié)機械手系統(tǒng)所對應(yīng)的鏈式空間內(nèi)，將各狀態(tài)變量的邊界條件約束與為了規(guī)避鏈式逆變換奇異位形的附加約束結(jié)合起來，可以得到一個擴展的非線性方程組：

(13)

若將控制輸入取為含有待定系數(shù)的多項式：

(14)

則邊界條件及運動總時間已知時，f1、f2、f3、h均為關(guān)于待定系數(shù)的表達式，非完整三關(guān)節(jié)機械手的運動規(guī)劃問題即可轉(zhuǎn)化成為式(13)所示的非線性方程組的求解問題。

可以采用牛頓迭代法求解非線性方程組(式(13))，如下所示：

c(k+1)=c(k)-[G′(c(k))]+G(c(k))

(15)

其中，c為控制輸入的待定系數(shù)，c=(c1,c2,c3,c4)T;G′(c)為G(c)的雅可比矩陣，G=(f1,f2,f3,h)T，[G′(c)]+為G′(c)的偽逆。

求解出鏈式空間中的控制輸入表達式后，便可規(guī)劃得鏈式空間中待控制狀態(tài)變量的運動軌跡，再通過鏈式逆變換將軌跡映射到關(guān)節(jié)空間中，得到非完整機械手三個關(guān)節(jié)角的運動軌跡。在此過程中鏈式空間內(nèi)對狀態(tài)變量附加的位形約束能夠保證規(guī)劃的軌跡可完全映射至關(guān)節(jié)空間中，從而規(guī)避了機械手的鏈式逆變換奇異位形。

4 仿真與實驗

增加位形約束后進行運動規(guī)劃，得到鏈式空間中各狀態(tài)變量的運動軌跡如圖5所示，此時z2始終保持為負值，這說明鏈式空間中規(guī)劃的軌跡可以完全地映射至關(guān)節(jié)空間中。通過鏈式逆變換求解出非完整機械手三個關(guān)節(jié)角的運動軌跡如圖6所示。

圖5 z的運動軌跡(加入位形約束后)Fig.5 The trajectory of z parameter (configuration constraint attached)

圖6 各關(guān)節(jié)角的運動軌跡(加入位形約束后)Fig.6 The trajectory of each joint (configuration constraint attached)

在非完整三關(guān)節(jié)機械手運動控制實驗平臺(圖7)開展實驗研究，利用兩個伺服電機作為非完整機械手的控制輸入，并通過UMAC控制器控制伺服電機的運動，通過角位移傳感器測量各關(guān)節(jié)角實際運動的角位移。非完整三關(guān)節(jié)機械手三個關(guān)節(jié)角的實際運動軌跡如圖8所示。

圖7 運動控制實驗平臺Fig.7 Motion control experimental platform

圖8 各關(guān)節(jié)角的實際運動軌跡Fig.8 The experimental trajectory of each joint

對比仿真與實驗結(jié)果，機械手三個關(guān)節(jié)角位移的運動趨勢基本相同，這表明本文提出的運動規(guī)劃方法可以實現(xiàn)對非完整三關(guān)節(jié)機械手的有效控制。

5 結(jié)論

(1)依據(jù)非完整機械手的結(jié)構(gòu)特征，將廣義坐標向量中的自由狀態(tài)變量分離出來，并利用邊界值作為待定系數(shù)進行求解，明確了一類可鏈式化機械手路徑規(guī)劃的基本方法。

(2)從非完整機械手的運動學模型及其變換特點出發(fā)，探討了運動規(guī)劃的方法以及存在奇異位形的原因——鏈式逆變換解的奇異性，并提出了保證鏈式逆變換可行的不等式約束條件。

(3)建立了一種可規(guī)避奇異位形的運動規(guī)劃方法，即利用罰函數(shù)將不等式約束轉(zhuǎn)化為等式約束后，將其與邊界條件約束結(jié)合起來形成一個非線性方程組，通過牛頓迭代法求解鏈式系統(tǒng)的控制輸入，規(guī)劃出一條從鏈式空間無奇異地映射到關(guān)節(jié)空間中的路徑。計算仿真和實驗表明所提出的運動規(guī)劃方法是有效的。

[1] DULEBA I, KHEFIFI W, KARCZ-DULEBA I. Layer, Lie Algebraic Method of Motion Planning for Nonholonomic Systems[J]. Journal of the Franklin Institute, 2012, 349(1):201-215.

[2] MURRAY R M, SASTRY S S. Steering Nonholonomic Systems in Chained Form[C]//IEEE Conference on Decision & Control. Brighton, 1991:1121-1126.

[3] TILBURY D, MURRAY R M, SASTRY S S.Trajectory Generation for the N-trailer Problem Using Goursat Normal Form[J]. IEEE Transactions on Automatic Control, 1995, 40(5):802-819.

[4] LOOCK W V, PIPELEERS G, DIEHL M, et al. Optimal Path Following for Differentially Flat Robotic Systems through a Geometric Problem Formulation[J]. IEEE Transactions on Robotics, 2014, 30(4): 980-985.

[5] JANIAK M, TCHONA K. Towards Constrained Motion Planning of Mobile Manipulators[C]//IEEE International Conference on Robotics and Automation. Anchorage， 2010:4990-4995.

[6] TAN T G, LI L, LIU M Y, et al. Design and Path Planning for Controllable Underactuated Manipulator[J]. International Journal of Advancements in Computing Technology, 2012, 4(2): 212-221.

[7] NAKAMURA Y, CHUNG W, SORDALEN O J. Design and Control of the Nonholonomic Manipulator[J]. IEEE Transactions on Robotics & Automation, 2001, 17(1):48-59.

[8] SORDALEN O J. Conversion of the Kinematics of a Car with N Trailers into a Chained Form[C]//IEEE International Conference on Robotics and Automation. Atlanta, 1993: 382-387.

[9] KORAYEM M H, NAZEMIZADEH M, RAHIMI H N. Trajectory Optimization of Nonholonomic Mobile Manipulators Departing to a Moving Target Amidst Moving Obstacles[J]. Acta Mechanica, 2013, 224(5):995-1008.

[10] TAN Y G, JIANG A Q, ZHOU Z D. A Nonholonomic Motion Planning and Control Based on Chained Form Transformation[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Beijing, 2006: 3149-3153.

[11] 譚躍剛. 非完整機器人的原理與控制[M].北京：科學出版社，2011:76-82. TAN Yuegang. Principle and Control of Nonholonomic Robots[M]. Beijing: Science Press, 2011:76-82.

(編輯 王旻玥)

Chained Form Conversion and Motion Planning Method for a Nonholonomic Manipulator

LI Liang1LIU Yifang2TAN Yuegang3

1.School of Mechatronic Engineering，Baoji University of Arts and Sciences，Baoji ，Shaanxi,7210072.China Chang Jiang Energy Corp.(Group)，Wuhan，4300703.School of Mechanical and Electronic Engineering，Wuhan University of Technology，Wuhan，430070

A motion planning algorithm was proposed to avoid the inverse chained form conversion singularity of nonholonomic three-joint manipulators. Free variables of generalized coordinates were separated according to the structural features of nonholonomic manipulators. Then the boundary values of variables acted as undetermined coefficients of control inputs. Inequality constraints were proposed to make sure that a trajectory in chained form spaces might be mapped into joint spaces for singularity of inverse chained form conversion. Additional constraints were used to avoid singular configuration of the inverse chained form conversion. Results of simulations and experiments prove the feasibility and effectiveness of this motion planning algorithm.

nonholonomic manipulator; chained form conversion; singular configuration; motion planning

2016-09-18

陜西省自然科學基礎(chǔ)研究計劃資助項目(2016JM5019)；陜西省教育科學“十三五”規(guī)劃資助項目(SGH16H161)；寶雞科技計劃資助項目(16RKX1-30)

TP24

10.3969/j.issn.1004-132X.2017.16.010

李 亮，男，1979年生。寶雞文理學院機械工程學院講師、博士。主要研究方向為機器人技術(shù)與控制。發(fā)表文章10余篇。E-mail：leeliang@126.com。劉一方，女，1990年生。中國長江動力集團有限公司汽輪機研究所工程師。譚躍剛，男，1959年生。武漢理工大學機電工程學院教授、博士研究生導(dǎo)師。