離軸拖掛輪式移動機器人跟蹤控制研究

苗盛陽，周宇生

（貴州大學數學與統計學院，貴陽 550025）

0 引言

輪式移動機器人是通過車輪轉動完成運動任務的一類特殊的移動機器人，具有結構簡單、運動靈活和能耗低等優點，廣泛應用于工業物料運輸和醫療等領域［1－3］。在眾多的輪式移動機器人中，有一類被稱為拖掛輪式移動機器人，它由多個單體輪式移動機器人連接而成［4－5］。根據相鄰2個移動機器人連接方式的不同，拖掛輪式移動機器人可分為軸上拖掛輪式移動機器人和離軸拖掛輪式移動機器人。軸上拖掛輪式移動機器人中相鄰兩車的連接點位于前車輪軸的中點［6－7］，而離軸拖掛輪式移動機器人中相鄰兩車的連接點位于前車尾桿的末端［8－11］。

軌跡跟蹤控制問題是拖掛輪式移動機器人控制中的一類重要問題，但復雜的結構、嚴重的欠驅動特性和非完整約束的影響，給拖掛輪式移動機器人的運動控制帶來了極大的困難［12－13］。針對軸上拖掛輪式移動機器人中掛車的軌跡跟蹤控制問題，文獻［14］基于李雅普諾夫函數設計了速度控制器，實現了掛車的位置跟蹤。針對軸上拖掛輪式移動機器人中拖車和掛車沿相同軌跡運動控制問題，文獻［15］通過增加控制輸入實現了對掛車的控制，并通過李雅普諾夫直接法設計了一種運動學控制器，實現了拖車和掛車沿同一軌跡運動。上述僅僅基于運動學模型設計控制器的方法雖然能實現軌跡跟蹤，但是沒有考慮機械系統的動力學模型，忽略了速度變化。而速度控制一般是通過力矩控制器來實現的，因此在控制設計過程中還要考慮動力學模型。動力學模型是描述速度與控制輸入力矩的變化關系的模型，可采用基于機械系統能量的歐拉-拉格朗日方程進行推導［16－17］。為了實現更加有效的控制，同時考慮位置跟蹤和速度跟蹤的雙環控制方法受到越來越多學者的青睞。雙環控制方法由內環和外環2個控制器組成，該方法可將不同的控制方法結合起來，充分發揮不同控制方法的優點。如文獻［16］提出了一種由模型預測控制方法和滑?？刂品椒ㄏ嘟Y合的雙環控制方法，該方法將模型預測控制的預測功能和滑?？刂频聂敯粜杂袡C結合起來，實現了掛車精確的軌跡跟蹤。文獻［18］提出了一種由基于李雅普諾夫函數的速度控制器和自適應力矩控制器組成的雙環控制方法，該方法在具有不確定因素的情況下實現了掛車的軌跡跟蹤控制。雙環控制方法雖然能實現控制目標，但為拖掛輪式移動機器人同時設計出2個合適的控制方法不是一件簡單的事情，并且上述研究忽視了移動機器人的潛在運動規律，難以精確地實現運動控制。想要實現拖掛輪式移動機器人的精確軌跡跟蹤，應充分挖掘潛在的運動規律，并將運動規律和控制器的設計有效地結合起來。如文獻［19］提出了一種動態軌跡跟蹤方法，該方法的核心是引入以目標軌跡曲線弧長來表示的相對曲率，通過跟蹤目標軌跡曲線的相對曲率來實現對軌跡的精確跟蹤。此外，對于軸上拖掛輪式移動機器人中掛車的軌跡控制問題，文獻［20］給出了兩車的運動軌跡曲線相對曲率的關系，并通過最優控制和滑?？刂品椒ㄍ瑫r實現了拖車和掛車的運動控制。由于非完整約束的影響，拖車和掛車很難沿著同一軌跡運動。為了實現拖車和掛車沿同一軌跡運動，文獻［21］為軸上拖掛輪式移動機器人中的掛車增添1個被動操舵角，在此基礎上推導出了拖車和掛車之間的速度關系，通過動態跟蹤方法實現了拖車和掛車沿同一軌跡運動。

然而上述結合運動規律的控制方法都是針對軸上拖掛輪式移動機器人提出的。與軸上拖掛輪式移動機器人相比，離軸拖掛輪式移動機器人的前車尾部有一段額外的連桿，后車與前車的連接點在上述連桿的末端，而軸上拖掛輪式移動機器人后車與前車的連接點在前車輪軸的中心，可以說軸上拖掛輪式移動機器人是離軸拖掛輪式移動機器人的一種特殊情形（即前車尾端連桿長度為零）。因此，離軸拖掛模型比軸上拖掛模型復雜，且由于連接點離前車的中心有一定的距離，使得其運動規律也更加復雜。上述2個原因導致離軸拖掛輪式移動機器人運動規律的相關研究較少。針對離軸拖掛輪式移動機器人中掛車的軌跡跟蹤控制問題，研究給出了拖車和掛車的運動軌跡滿足的一個重要的曲率關系，利用此關系將掛車的軌跡跟蹤問題轉化為拖車的軌跡跟蹤問題，并利用動態跟蹤方法設計出比例積分反饋控制器，實現了掛車的精確軌跡跟蹤。

1 問題描述

下面將介紹離軸拖掛輪式移動機器人的結構及其運動學和動力學模型。

1.1 結構介紹

如圖1所示，離軸拖掛輪式移動機器人由前面的拖車、中間的剛體連桿以及跟在后面的掛車組成。通常情況下，拖車含有2個主動的驅動輪和1個被動的萬向輪。拖車在驅動輪的轉動下實現自主移動，掛車一般不含驅動裝置，只能在拖車的拉拽下完成運動。為了方便起見，將拖車中左右2個驅動輪的控制輸入力矩分別記作τl、τr；設拖車的質心為點A，坐標為（x1，y1），掛車的質心為點B，坐標為（x2，y2）；兩桿的連接點為點C，坐標為（x3，y3）。拖車的運動方向與X軸正方向的夾角為θ1，掛車的運動方向與X軸正方向的夾角為θ2。離軸拖掛輪式移動機器人的廣義坐標為q＝［x1，y1，θ1，θ2］T，所以離軸拖掛輪式移動機器人是1個4自由度2輸入的欠驅動系統?？刂颇繕耸窃谕宪嚿鲜┘雍线m的控制輸入，在拖車的拖拽作用下實現掛車跟蹤目標軌跡曲線。此外，研究所使用的狀態變量如表1所示。

表1 模型的主要參數

圖1 離軸拖掛輪式移動機器人示意圖

1.2 運動學模型

將V＝［v1，w1］T看作離軸拖掛輪式移動機器人的輸入向量，其中v1表示拖車的前向速度，w1表示拖車的偏航轉速。同時，用v2表示掛車的前向速度，w2表示掛車的偏航轉速。假設離軸拖掛輪式移動機器人在運動過程中車輪不發生側滑，則A點和B點的橫向速度為零，得到的非完整約束為：

式（1）可寫成矩陣的形式：

其中，

并且，A點和B點的前向速度可表示為：

結合式（1）和式（3）可得：

此外，A點和B點的位置關系可表示為

對式（6）進行關于時間t的求導，可得：

結合式（4）、式（5）、式（7）可得：

綜上所述，得到的運動學方程為：

該運動學方程也可用矩陣表示為：

其中，

根據上述分析，可進一步得到：

1.3 動力學模型

為了利用歐拉-拉格朗日方程計算動力學方程，應計算出機械系統各個部分的能量。假設離軸拖掛輪式移動機器人始終在平坦地面運動，可忽略系統的勢能，因此系統的總能量等于總動能。首先計算出2段連桿的總能量，假設連桿為輕質剛體連桿，質量忽略不計，則連桿的總能量為K1＝0。

其次，拖車車身及其車輪的總能量可表示為：

此外，根據式（6），掛車車身及其車輪的總能量可表示為：

因此，拉格朗日函數L就可寫為3部分能量的總和：

其具體表達式為：

根據非完整機械系統的歐拉－拉格朗日方程，離軸拖掛輪式移動機器人的動力學方程可寫成：

按照狀態變量的各階導數，可將歐拉－拉格朗日方程進一步整理成：

式中：M（q）為慣性矩陣；C為柯氏力矩陣；T為控制輸入力矩；X（q）為控制輸入力矩矩陣；λ為拉格朗日乘子。

其中，

由于機械系統在運動過程中不可避免地會受到外界環境干擾D1的影響，因此在式（21）等號兩邊同乘ST（q）可得：

矩陣M1、C1、T1、D1的具體形式如下：

2 運動規律

在控制過程中，離軸拖掛輪式移動機器人的自由度有4個，而控制輸入只有來自拖車車輪的2個控制輸入力矩，這給離軸拖掛輪式移動機器人的控制帶來了困難。若能找出拖車和掛車滿足的運動規律，則可通過設計控制器來實現拖車的控制，并根據兩車滿足的運動規律間接地實現對掛車的控制。

定理1：兩車的前向速度和偏航轉速滿足如下關系。

證明：根據幾何關系，A、B、C3點的坐標滿足如下的關系。

求式（32）兩式的平方和，可得：

所以拖車的前向速度v1可表示為：

當剛體連桿繞著某點轉動時，剛體連桿上的每一點的角速度都是相等的，但線速度不相等，線速度會隨著該點與轉動中心的距離的不同而不同。如圖1所示，由于離軸拖掛輪式移動機器人的兩桿是剛體連桿，運動時同一桿上的任意2點的偏航角和偏航轉速相等，因此A點和C點的偏航角滿足θ1＝θ3。根據文獻［20］給出的軸上拖掛輪式移動機器人拖車和掛車偏航轉速關系可得：

所以拖車和掛車的速度之間的關系滿足：

由式（36）可知，拖車的前向速度和偏航轉速可由掛車的前向速度和偏航轉速得到，但根據式（36）很難把v2、w2的具體表達式反解出來，所以結合運動學方程，將w2代入式（33）可得：

因此，拖車與掛車之間的速度關系還可表示為：

定理2：拖車運動軌跡的相對曲率k1和掛車運動軌跡的相對曲率k2滿足如下的關系。

證明：結合式（34）和式（35）可得，

式中：k1和k2分別為拖車和掛車的運動軌跡曲線的相對曲率。根據式（39），可將相對曲率為k2的掛車運動軌跡轉化為相對曲率為k1的拖車運動軌跡，因此掛車的軌跡跟蹤控制問題可轉化為拖車的軌跡跟蹤控制問題。換言之，要實現掛車精確地跟蹤相對曲率為k2的軌跡曲線，只需要設計2個控制輸入力矩。

3 動態跟蹤方法和控制器設計

下面將介紹動態跟蹤方法以及控制器的設計過程，具體的控制流程如圖2所示。

圖2 控制流程圖

3.1 動態跟蹤方法

若r1＝（x1r，y1r）是拖車的運動軌跡跟蹤目標，則根據運動學方程可得：

對式（41）進行關于t的求導，可得：

結合式（41）和式（42）可得：

所以，拖車的速度跟蹤目標（v1r，w1r）和軌跡跟蹤目標（x1r，y1r）滿足：

式中：k1r（s（t））為目標軌跡曲線的相對曲率，并且為軌跡曲線的弧長，并且

根據式（41）和式（44）可得，拖車的前向速度目標v1r和偏航轉速目標w1r可由拖車的軌跡跟蹤目標（x1r，y1r）確定；反之，軌跡跟蹤目標（x1r，y1r）可由前向速度目標v1r和偏航轉速目標w1r得到。對于掛車的運動軌跡跟蹤目標r2＝（x2r，y2r），同樣可得到相似的結論：

因此，利用式（45）可將軌跡跟蹤目標r2＝（x2r，y2r）轉化為速度跟蹤目標（v2r，w2r），并利用文獻［19］提出的動態跟蹤方法，將前向速度目標設計為一個合適的函數，即：

式中：l為目標軌跡曲線的長度；β為合適的參數，該參數可根據實際需要進行調整。偏航轉速目標通過以弧長表示的目標軌跡曲線相對曲率和前向速度的乘積得到，具體形式如下：

式中：k2r（s（t））為目標軌跡曲線的相對曲率。

3.2 控制器設計

與軸上拖掛輪式移動機器人相比，離軸拖掛輪式移動機器人的動力學模型更復雜，為了實現更精確的速度跟蹤，可采用比例積分反饋控制方法?；趧恿W方程（22），設計比例積分反饋控制器來實現速度跟蹤?？刂破骺蓪懗上旅娴男问剑?/p>

式中：KP和KI分別為比例項矩陣和積分項矩陣，它們都是正定的常值矩陣；Kt為魯棒項矩陣；Ve＝Vc－V。將式（48）代入式（22），可得：

基于上述分析，構造的李雅普諾夫函數V為：

根據M1和KI的正定性，可得V也是正定的，再對V進行關于時間t的求導，可進一步得到：

根據李雅普諾夫穩定性理論，V≥0，≤0，因此系統是穩定的。

4 數值仿真

為了驗證所提控制方法的有效性，利用Matlab進行仿真實驗。根據式（22）可得，當拖車的運動軌跡是半徑為的圓時，掛車的運動軌跡是1個單位圓。換言之，要實現掛車跟蹤目標軌跡曲線r2＝（cost，sint），僅需要控制拖車跟蹤曲線根據上述的分析，當L1＜L2時，拖車的運動軌跡為1個半徑大于1的圓軌跡；當L1＝L2時，拖車與掛車沿同一單位圓運動；當L1＞L2時，拖車的運動軌跡為1個半徑小于1的圓軌跡。因此，利用動態跟蹤方法，將動態跟蹤目標設計為：

其中，k1r（s（t））可表示為：

此外，離軸拖掛輪式移動機器人及其相關的參數取值為：r＝0.05 m，β＝0.5，m1＝2 kg，l＝2πm2＝1.5 kg，mw＝0.2 kg，b＝0.2 m，Iw＝0.01 kgm2，I1＝0.1 kgm2，I2＝0.05 kgm2，比例項矩陣KP＝10I2，積分項矩陣KI＝10I2，魯棒項矩陣Kt＝0.5I2，其中I2為二階單位矩陣。在仿真過程中，總仿真時間設定為20 s。由于離軸拖掛輪式移動機器人在實際環境下運動的過程中不可避免地會受到外界干擾，因此外界干擾選為（td1，td2）＝（0.1sint，0.1cost）。離軸拖掛輪式移動機器人前后兩桿的長度是控制過程中的重要參數，會直接影響到拖車和掛車的運動軌跡。當L1＜L2時，兩桿的長度選擇為L1＝0.1m，L2＝0.6 m；當L1＞L2時，兩桿的長度選擇為L1＝0.5 m，L2＝0.3 m；當L1＝L2時，兩桿的長度選擇為L1＝0.3 m，L2＝0.3 m。

3種情況下離軸拖掛輪式移動機器人的軌跡跟蹤情況如圖3—圖5所示，其中黑色實線表示目標軌跡曲線，紅色虛線表示拖車的運動軌跡曲線，綠色虛線表示掛車的運動軌跡曲線?？煽闯?，掛車可沿目標軌跡運動。

圖3 掛車的軌跡跟蹤（L1＝0.1 m，L2＝0.6 m）

圖4 掛車的軌跡跟蹤（L1＝0.5 m，L2＝0.3 m）

圖5 掛車的軌跡跟蹤（L1＝0.3 m，L2＝0.3 m）

同時，為驗證所提力矩控制器的有效性，3種情況下拖車的前向速度跟蹤誤差v1e和偏航轉速跟蹤誤差w1e隨時間變化的仿真結果如圖6—圖11所示?？煽闯?，拖車的前向速度跟蹤誤差v1e和偏航轉速跟蹤誤差w1e始終在零值附近的微小范圍內變化。特別是偏航轉速跟蹤誤差，始終在－0.02～0.02 rad／s的微小范圍內變化。

圖6 拖車的前向速度跟蹤誤差（L1＝0.1 m，L2＝0.6 m）

圖7 拖車的偏航轉速跟蹤誤差（L1＝0.1 m，L2＝0.6 m）

圖8 拖車的前向速度跟蹤誤差（L1＝0.5 m，L2＝0.3 m）

圖9 拖車的偏航速度跟蹤誤差（L1＝0.5 m，L2＝0.3 m）

圖10 拖車的前向速度跟蹤誤差（L1＝0.3 m，L2＝0.3 m）

圖11 拖車的偏航速度跟蹤誤差（L1＝0.3 m，L2＝0.3 m）

從理論上來講，由于偏航轉速誤差始終穩定在一個趨近于零的小范圍內，表明所提控制策略對目標軌跡曲線的相對曲率跟蹤誤差非常小，掛車可很好地跟蹤目標軌跡曲線。從圖3—圖5也可看出，3種情況下的掛車在拖車的拉拽下可精確地跟蹤目標軌跡曲線，其運動軌跡與目標軌跡幾乎重合。

5 結論

1）在軸上拖掛輪式移動機器人運動規律的基礎上給出了拖車和掛車之間的速度關系以及兩車運動軌跡曲線的相對曲率之間的關系，將掛車的控制問題轉化為拖車的控制問題，極大地降低了控制器的設計難度。

2）運用動態跟蹤方法跟蹤了目標軌跡曲線的相對曲率，大大增加了軌跡跟蹤的精確性。

3）通過設計作用在拖車上的2個控制輸入力矩實現了掛車的控制。

4）所提比例積分反饋控制器可同時實現前向速度和偏航轉速的控制，為移動機器人的動力學控制器設計提供了一種新的思路。