四自由度模擬飛行復(fù)合平臺(tái)運(yùn)動(dòng)學(xué)設(shè)計(jì)及仿真

董艇艦，李 化，桑 超

（中國民航大學(xué)航空工程學(xué)院，天津 300300）

1962年美國工程師Cappel 獨(dú)立設(shè)計(jì)了用于飛行模擬器的六自由度運(yùn)動(dòng)平臺(tái)[1]。國內(nèi)哈爾濱工業(yè)大學(xué)、燕山大學(xué)等高校陸續(xù)開展了六自由度飛行模擬器平臺(tái)的相關(guān)研究[2]。但其存在體積龐大、工作空間小、控制困難及成本較高等缺點(diǎn)，因此根據(jù)飛機(jī)實(shí)際飛行狀態(tài)（沿飛機(jī)橫軸方向的移動(dòng)可以忽略，沿縱軸方向的移動(dòng)可以通過視景系統(tǒng)的相對(duì)運(yùn)動(dòng)原理實(shí)現(xiàn)），得到一個(gè)四自由度的復(fù)合平臺(tái)，隸屬于層疊式運(yùn)動(dòng)平臺(tái)，應(yīng)用于低于真實(shí)模擬機(jī)運(yùn)動(dòng)平臺(tái)、高于普通游戲機(jī)運(yùn)動(dòng)平臺(tái)的模擬飛行體驗(yàn)和多方位展臺(tái)。

1 運(yùn)動(dòng)學(xué)分析

正向運(yùn)動(dòng)學(xué)是指已知各連桿的幾何參數(shù)和機(jī)構(gòu)各關(guān)節(jié)變量，求末端執(zhí)行器的坐標(biāo)位姿；反向運(yùn)動(dòng)學(xué)則是已知末端執(zhí)行器相對(duì)于參考坐標(biāo)系的位姿，求出機(jī)器人能夠達(dá)到該預(yù)期位姿的關(guān)節(jié)變量值[3]。串聯(lián)機(jī)構(gòu)正解較容易，反解卻相當(dāng)復(fù)雜，原因在于其多解性[4-5]。但該設(shè)計(jì)平臺(tái)各關(guān)節(jié)軸線相互垂直，逆向求解相對(duì)容易。

1.1 結(jié)構(gòu)參數(shù)建模

利用Solidworks 對(duì)四自由度復(fù)合平臺(tái)進(jìn)行裝配圖構(gòu)建，如圖1 所示。

其工作原理為：兩個(gè)轉(zhuǎn)動(dòng)副控制平臺(tái)俯仰和滾轉(zhuǎn)運(yùn)動(dòng)，回轉(zhuǎn)軸承控制平臺(tái)偏航運(yùn)動(dòng)，剪升裝置控制平臺(tái)升降運(yùn)動(dòng)。由于其偏航角度達(dá)到360°，故該平臺(tái)能夠?qū)崿F(xiàn)模擬直升機(jī)旋轉(zhuǎn)拍攝和軍機(jī)360°掃射的狀態(tài)。

該平臺(tái)主要技術(shù)參數(shù)參照文獻(xiàn)[6-7]，如表1 所示。

圖1 四自由度復(fù)合平臺(tái)三維模型Fig.1 3D model of four-degree-of-freedom composite platform

表1 運(yùn)動(dòng)平臺(tái)技術(shù)指標(biāo)Tab.1 Sports platform technical indicators

對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)分析。首先簡化圖1 所示的模型，移動(dòng)關(guān)節(jié)的移動(dòng)方向和轉(zhuǎn)動(dòng)關(guān)節(jié)的轉(zhuǎn)軸定義為z軸，沿該連桿與前一個(gè)連桿z 軸的公垂線方向建立x軸，末端執(zhí)行器的坐標(biāo)與最后一個(gè)關(guān)節(jié)坐標(biāo)同等位姿，得到如圖2 所示的坐標(biāo)系。

圖2 四自由度復(fù)合平臺(tái)坐標(biāo)建立Fig.2 Establishment of four-degree-of-freedom composite platform coordinates

Denavit-Hartenberg 提出的D-H 參數(shù)模型可滿足機(jī)構(gòu)中的最小線性表示[5]，用4 個(gè)參數(shù)就能描述坐標(biāo)變換：沿x 軸平移距離a 為連桿長度；繞x 軸旋轉(zhuǎn)角度α 為扭角；沿z 軸平移距離d 為連桿偏距；繞z 軸旋轉(zhuǎn)角度θ 為關(guān)節(jié)轉(zhuǎn)角。故根據(jù)上圖及平臺(tái)尺寸得到四自由度復(fù)合平臺(tái)D-H 連桿參數(shù)，如表2 所示。表2 中：d1，θ2，θ3，θ4為變量，其變動(dòng)范圍分別為30～130 mm、0°～360°、-40°～40°、-40°～40°。初始值分別為30 mm、90°、90°、0°。

表2 D-H 連桿參數(shù)表Tab.2 D-H parameters

由此得到四自由度復(fù)合平臺(tái)總變換矩陣為

其中：Rot（）表示繞坐標(biāo)軸z 的旋轉(zhuǎn)矩陣，Trans（）表示沿坐標(biāo)軸的平移變換矩陣。將以上連桿參數(shù)代入式（1），得到各個(gè)連桿的變換矩陣，即

以上4 個(gè)矩陣連乘即得到坐標(biāo)系{4}相對(duì)于坐標(biāo)系{1}的變換矩陣

其中：三維矩陣[n o a]表示運(yùn)動(dòng)平臺(tái)的姿態(tài)；n，o，a 分別為變換后坐標(biāo)系x、y、z 軸相對(duì)于原坐標(biāo)系的方向列陣；p 為平臺(tái)坐標(biāo)原點(diǎn)相對(duì)基坐標(biāo)系的空間位置，其內(nèi)部正運(yùn)動(dòng)學(xué)方程為

1.2 反向運(yùn)動(dòng)學(xué)求解

該運(yùn)動(dòng)平臺(tái)各坐標(biāo)系之間的旋轉(zhuǎn)角度均為90°的倍數(shù)，且觀察正解公式可知，用歐拉變換法進(jìn)行反向求解相對(duì)簡單，即

根據(jù)式（3），分別求4 個(gè)關(guān)節(jié)變量：d1，θ2，θ3，θ4。

1.2.1 d1求解

由pz正向運(yùn)動(dòng)學(xué)公式可直接得出移動(dòng)變量

d1= pz- d2- d3sin θ3- d4sin θ3cos θ4，其中θ3、θ4已知且唯一，故d1也有唯一解。

至此，四自由度復(fù)合平臺(tái)反向運(yùn)動(dòng)學(xué)求解完畢。等待仿真驗(yàn)證其正確性。

1.2.2 θ2求解

式中

由式（5）得

由式（6）矩陣中的第3 行第3 列對(duì)應(yīng)相等得到

得出θ2=arctan），θ2∈[0°，360°]，根據(jù)反正切函數(shù)圖像可知在該范圍內(nèi)有兩個(gè)解：當(dāng)為正值時(shí)兩個(gè)解的區(qū)間分別為（0°，90°）和（180°，270°）；為負(fù)值時(shí)，兩個(gè)解的區(qū)間分別為（90°，180°）和（270°，360°）。

1.2.3 θ3求解

由az正向運(yùn)動(dòng)學(xué)方程得出θ3= -arccos az。θ3∈[-40°，40°]，根據(jù)反余弦函數(shù)圖像可知在該范圍內(nèi)有唯一解。

1.2.4 θ4求解

正向運(yùn)動(dòng)學(xué)方程中的oz和nz兩式相除可得

求得θ4=-arctan），θ4∈[-40°，40°]，根據(jù)反正切函數(shù)圖像可知在該范圍內(nèi)有唯一解。

2 四自由度復(fù)合平臺(tái)仿真分析

在數(shù)學(xué)計(jì)算基礎(chǔ)上通過ADAMS 建立或?qū)霂缀文Ｐ蛯?duì)四自由度運(yùn)動(dòng)平臺(tái)，進(jìn)行運(yùn)動(dòng)學(xué)仿真，利用Matlab Robotics Toolbox 工具箱建立數(shù)學(xué)模型，用于仿真運(yùn)動(dòng)學(xué)正反解，包括建立數(shù)學(xué)模型、運(yùn)動(dòng)學(xué)正逆分析以及基于關(guān)節(jié)空間規(guī)劃和笛卡爾空間規(guī)劃的運(yùn)動(dòng)軌跡規(guī)劃及仿真驗(yàn)證等問題。Matlab Robotics Toolbox工具箱是由澳大利亞科學(xué)家Peter Corke 開發(fā)的一套基于Matlab 的機(jī)器人工具箱[3]，該工具箱提供了機(jī)器人研究中很多重要的函數(shù)，如機(jī)器人運(yùn)動(dòng)學(xué)、軌跡規(guī)劃等[8]。

2.1 數(shù)學(xué)模型

用Matlab Robotics Toolbox 工具箱進(jìn)行運(yùn)動(dòng)學(xué)仿真，首先需建立數(shù)學(xué)模型。使用Link 函數(shù)創(chuàng)建連桿，將各個(gè)連桿用函數(shù)連起來構(gòu)成平臺(tái)模型。創(chuàng)建連桿一般有標(biāo)準(zhǔn)D-H 法（standard）和改進(jìn)D-H 法（modified）[9]，選擇標(biāo)準(zhǔn)D-H 法建模，利用表2 中的平臺(tái)關(guān)節(jié)變量，在Matlab 編輯器中輸入以下代碼完成四自由度復(fù)合平臺(tái)的數(shù)學(xué)建模：

robot=SerialLink（[L1 L2 L3 L4]，′name′，‘四自由度復(fù)合平臺(tái)’）；

為了直觀展示，通常運(yùn)用plot 函數(shù)輸出該數(shù)學(xué)模型，如圖3 所示。以棍棒形式簡化實(shí)體模型，顯示在三維坐標(biāo)系中，達(dá)到簡單易懂的視覺效果。

圖3 四自由度復(fù)合平臺(tái)數(shù)學(xué)模型Fig.3 Mathematical model of four-degree-of-freedom composite platform

2.2 平臺(tái)運(yùn)動(dòng)學(xué)仿真及驗(yàn)證

基于平臺(tái)數(shù)學(xué)模型，利用Robotics Toolbox 中的fkine和ikine 函數(shù)進(jìn)行運(yùn)動(dòng)學(xué)正向和逆向仿真，對(duì)平臺(tái)運(yùn)動(dòng)學(xué)公式進(jìn)行驗(yàn)證，兩個(gè)函數(shù)調(diào)用格式為

其中：ROBOT 為該平臺(tái)構(gòu)建數(shù)學(xué)模型的名稱；Q 為關(guān)節(jié)空間內(nèi)的角位移向量[10]；T 為反解的矩陣。當(dāng)反解的機(jī)構(gòu)對(duì)象自由度少于6 時(shí)，要用M 來忽略某個(gè)關(guān)節(jié)自由度。

隨機(jī)選取6 個(gè)關(guān)節(jié)參數(shù)值，利用正向運(yùn)動(dòng)學(xué)公式（式（2））得到相應(yīng)末端執(zhí)行器的位姿矩陣，將該矩陣與對(duì)應(yīng)的fkine 函數(shù)所得出的矩陣對(duì)比，矩陣中參數(shù)一致后將該矩陣作為反向運(yùn)動(dòng)學(xué)公式的輸入，驗(yàn)證求得關(guān)節(jié)角度值與初始角度值的一致性。隨機(jī)選取3 組實(shí)驗(yàn)數(shù)據(jù)，第1 組實(shí)驗(yàn)數(shù)據(jù)關(guān)節(jié)參數(shù)分別為：30 mm、90°、90°、30°，通過正解公式和Matlab 函數(shù)求解所得末端位姿T1均一致。然后利用反向運(yùn)動(dòng)學(xué)公式和ikine函數(shù)反解出關(guān)節(jié)空間內(nèi)的角位移向量Q1，發(fā)現(xiàn)該組關(guān)節(jié)參數(shù)值與初始角度一致（Matlab 中角度以弧度形式表示）。

第2 組隨機(jī)選取的關(guān)節(jié)數(shù)據(jù)分別為：50 mm、45°、30°、10°，方法同上，得到該組末端位姿T2以及反向運(yùn)動(dòng)學(xué)公式和函數(shù)反解出的關(guān)節(jié)空間內(nèi)的角位移向量Q2，即

第3 組隨機(jī)選取的關(guān)節(jié)數(shù)據(jù)分別為：100 mm、180°、-15°、5°，方法同上，得到該組末端位姿T3以及反向運(yùn)動(dòng)學(xué)公式和函數(shù)反解出的關(guān)節(jié)空間內(nèi)的角位移向量Q3，即

3 組隨機(jī)實(shí)驗(yàn)證明了正逆解公式的正確性、Matlab 建立數(shù)學(xué)模型的實(shí)用性及求解的可行性。

3 軌跡規(guī)劃

關(guān)節(jié)空間坐標(biāo)系軌跡規(guī)劃是各關(guān)節(jié)的位置、速度、加速度以時(shí)間為自變量的軌跡變化，為后期控制設(shè)計(jì)提供參考。利用Robotics Toolbox 提供的jtraj 函數(shù)可實(shí)現(xiàn)關(guān)節(jié)空間規(guī)劃。該平臺(tái)設(shè)計(jì)初始和終止位置速度為0，故選用三次多項(xiàng)式進(jìn)行軌跡規(guī)劃，在命令窗口輸入各關(guān)節(jié)初始參數(shù)和終止參數(shù)代碼，即

得到第1 個(gè)移動(dòng)關(guān)節(jié)軌跡圖像，如圖4 所示。

第2、3、4 關(guān)節(jié)為轉(zhuǎn)動(dòng)關(guān)節(jié)，由于轉(zhuǎn)動(dòng)關(guān)節(jié)變化相似，取第3 關(guān)節(jié)做軌跡圖像，如圖5 所示。

圖4 移動(dòng)關(guān)節(jié)曲線（關(guān)節(jié)1）Fig.4 Moving joint curve（Node One）

圖5 轉(zhuǎn)動(dòng)關(guān)節(jié)曲線（關(guān)節(jié)3）Fig.5 Rotating joint curve（Node Three）

由圖4～圖5 可以看出，位移、速度、加速度曲線連續(xù)平滑，證明了仿真的可行性以及平臺(tái)4 個(gè)關(guān)節(jié)空間的實(shí)際運(yùn)動(dòng)情況切實(shí)可行，為對(duì)四自由度復(fù)合平臺(tái)的控制設(shè)計(jì)提供了理論依據(jù)。

4 結(jié)語

對(duì)四自由度模擬飛行運(yùn)動(dòng)平臺(tái)進(jìn)行了運(yùn)動(dòng)學(xué)分析和仿真以及軌跡規(guī)劃，得到結(jié)論如下：

1）用D-H 參數(shù)法對(duì)該平臺(tái)模型進(jìn)行坐標(biāo)系和DH 參數(shù)表格建立，得到四自由度復(fù)合平臺(tái)正解求解通式。用歐拉變換法對(duì)平臺(tái)模型進(jìn)行反解計(jì)算，得到4個(gè)關(guān)節(jié)的反解求解通式。分別代入初始值得出正確結(jié)果。

2）用Matlab Robotics Toolbox 工具箱對(duì)平臺(tái)進(jìn)行數(shù)學(xué)建模、正反解仿真，仿真結(jié)果與推導(dǎo)通式計(jì)算結(jié)果一致，說明所求正反解通式及仿真模型的正確性，驗(yàn)證了平臺(tái)參數(shù)的合理性。

3）最后用Matlab 進(jìn)行軌跡規(guī)劃仿真，所得曲線光滑平穩(wěn)，驗(yàn)證了四自由度復(fù)合平臺(tái)的可行性。