六自由度飛行姿態(tài)仿真平臺設計

杜航航

（中國民航大學工程技術訓練中心，天津 300300）

飛行模擬器是飛機子系統(tǒng)性能驗證的載體，其中六自由度運動平臺是飛行模擬器的重要組成部分。飛機復雜運動狀態(tài)模擬過程中，六自由度運動平臺可提供滾轉、俯仰、偏航以及3 個方向的平移運動[1]。

為便于模擬飛行姿態(tài)變化，德國工程師Stewart[2]于1965年設計了六自由度并聯(lián)機構。與串聯(lián)機構相比，六自由度并聯(lián)機構具有結構緊湊、多自由度、高剛性、高精度、誤差不疊加等優(yōu)點[3-4]。六自由度平臺運動學分析主要集中于正解與反解問題。其中，并聯(lián)機構運動學正解是已知關聯(lián)機構6 個伸縮桿的長度，求解平臺的位姿；而反解過程是根據(jù)預期位姿逆向求解伸縮桿的長度。王曉飛等[5]利用高精度光柵尺得到驅動桿的長度，進而利用牛頓迭代法進行正解運算。張鑫[6]使用D-H 表示法建立機械結構的空間坐標系，結合齊次坐標變換建立位姿反解方程。李晶[7]基于遺傳神經(jīng)網(wǎng)絡進行正解運算，仿真驗證該方法具有較高的收斂性。目前常用的正解方法包括解析法與數(shù)值法。由于正解方程組具有高度非線性，導致結果不唯一，僅適用于理論分析，實踐中多采用反解分析方法進行設計[8]。此外，正解算法計算較復雜，目前僅局限于求解速度與精度。而反解算法較為成熟，使用廣泛，但已有研究通常以驅動桿為對象，執(zhí)行器多采用質量大、價格高的液壓、氣動、電動伺服等驅動器，導致成品體積大、控制復雜、不易維護[9]。

針對上述不足，設計了以舵機為驅動器的低成本桌面型并聯(lián)六自由度姿態(tài)仿真平臺。通過運動學反解，首先求解驅動桿長度，進而通過空間幾何解析求得舵機角度變化。該方案將驅動桿的伸縮運動轉化為舵機的旋轉運動，實現(xiàn)了六自由度平臺結構的小型化。

1 運動學反解算法

1.1 空間坐標系建立

并聯(lián)六自由度平臺由上下兩個平臺及6 條支鏈組成，每條支鏈由驅動器、驅動桿及連接鉸鏈構成，Stewart 結構如圖1所示。通過6 根驅動桿的協(xié)同軸向伸縮實現(xiàn)上移動平臺6 個自由度的運動。為便于分析計算，在上移動平臺建立活動坐標系，在下基座平臺建立靜坐標系，將支鏈與上平臺鉸鏈點分別標記為A1～A6，支鏈與下平臺鉸鏈點對應標記為B1～B6。初始位置時，平臺處于中位，x 軸及x′軸分別垂直于B4B5及A1A2的連線，z 及z′軸垂直向上，根據(jù)右手螺旋法可判定y 及y′軸方向。設上平臺A1A2、A3A4、A5A6相鄰鉸點距離為a,下平臺B1B2、B3B4、B5B6距離為b,上平臺各鉸點外接圓半徑為r，下平臺各鉸點外接圓半徑為R，上下平臺初始距離為h。為簡化公式，記P =

圖1 Stewart 六自由度平臺示意圖Fig.1 Diagram of six-DOF structure

上平臺中，A1坐標旋轉120°得到A3坐標；下平臺中，B1坐標旋轉120°得到B3坐標，如此類推，經(jīng)坐標系旋轉變換公式可得上下鉸點坐標值，令G 為O 到O′的距離，即

1.2 反解運算

Stewart 型結構上移動平臺的位姿變化通過6 根驅動桿的線性伸縮來實現(xiàn)。要求解6 根驅動桿的實時長度，只需將上下平臺各鉸接點坐標轉化到同一坐標系下，再計算兩點間距離。動坐標系{O′}（O′x′y′z′)相對于參考坐標系{O}位置和姿態(tài)可通過有限次數(shù)的平移變換與旋轉變換得到。

將坐標系{O′}在x、y、z 軸方向上分別平移dx、dy、dz 距離，3 次平移變換矩陣分別為T1、T2、T3，然后依次進行3 次旋轉變換，繞z 軸、y 軸、x 軸分別旋轉α、β、γ角度，3 次旋轉變換矩陣分別記為T4、T5、T6?？傻脛幼鴺讼档絽⒖甲鴺讼档凝R次坐標變換矩陣T，為

其中：sα=sin α，cα=cos α，sβ、cβ、sγ、cγ 同理。已知某坐標系中任意一點的坐標值，可通過左乘齊次坐標變換矩陣T，將該點位置向量變換到另外一個坐標系中。將上移動平臺各鉸點變換后齊次坐標記為Pi=（Pix，Piy，Piz，1），通過齊次坐標變換，可得到靜坐標系下的坐標Ai=（Aix，Aiy，Aiz，1），i=1～6。則有

在下基座平臺中，設各鉸點坐標為Bi=（Bix，Biy，Biz），在同一參考坐標系下，驅動桿實時長度（即上下鉸點間的距離）可由AiBi的長度計算求得，即

由以上過程可解算六根驅動桿的實時長度，由此完成了運動學反解過程。

1.3 舵機角度計算

舵機連臂通過萬向球頭連桿連接上下鉸點，反解過程已得到Li，選用舵機為執(zhí)行機構，通過控制轉動角度實現(xiàn)上下平臺對應鉸點間距離的變化，求解驅動桿伸縮量變化就轉化為求解舵機角度θ。根據(jù)反解運算過程，可獲取上平臺6 個鉸點在參考坐標系中的坐標，進而通過幾何解析法求出各舵機轉動角度。由于萬向球頭的存在，連桿與舵機連臂所在平面與舵機軸轉動方向不重合，簡化的空間結構如圖2所示。

在空間內(nèi)將F 點投射到下平臺,如圖3所示，可計算F 點坐標值，設上平臺鉸點P 動態(tài)坐標為（Px，Py，Pz），根據(jù)向量OB+BF+FP+PO′+O′O=0 運算將各點坐標代入，利用|PB|=L，則有

|cos φ·OF+sin φ·cos θ·BF-Px′-sin φ·OF+

圖2 舵機連桿結構示意圖Fig.2 Diagram of servo connecting rod

將上述公式展開，令

其中：V 為可以實時計算的已知量，則可得到

求解該一元二次方程并求解角度θ，可得

圖3 F 點在參考坐標系中的投影Fig.3 Projection of point F in reference coordinate system

2 Solidworks 仿真設計

在Solidworks 中建立六自由度姿態(tài)仿真平臺模型，如圖4所示。執(zhí)行機構全部按實際參數(shù)設計，通過軟件裝配體功能，將各零件模型通過相關約束關系進行裝配。軟件可控制某一舵機的轉動角度，分析6 條驅動桿運動間的耦合，以及位姿調(diào)節(jié)實現(xiàn)范圍，設計參數(shù)及6 個自由度可調(diào)節(jié)范圍分別如表1和表2所示。

圖4 六自由度平臺仿真模型Fig.4 Simulation model of six-DOF

表1 六自由度平臺主要運動參數(shù)Tab.1 Main motion parameter of six-DOF

表2 平臺位姿變化范圍Tab.2 Motion range of six-DOF

3 運動平臺控制系統(tǒng)設計

運動平臺控制系統(tǒng)是上位機解算單元與平臺執(zhí)行機構的連接部分，控制系統(tǒng)如圖5所示。上位機通過運動學反解將所對應的位姿數(shù)據(jù)轉化為舵機角度值發(fā)送給控制系統(tǒng)，控制系統(tǒng)根據(jù)接收到的6 個舵機角度值控制相應舵機轉動。

圖5 控制系統(tǒng)框圖Fig.5 Block diagram of control system

控制板核心處理器為STM32F103 單片機，最大速度為72 MHz，主要用于接收指令控制舵機轉動，運動學反解及舵機角度解算任務全部在PC 端完成。這種處理方式解放了下位機的運算解析過程，上位機以10 Hz的高頻率向控制板發(fā)送角度指令，舵機轉動實時性好，平臺運動過程無卡頓情況。

4 平臺測試

4.1 輸入輸出響應測試

由上位機軟件在x 軸方向上輸入x=10cos（0.1πt）位姿曲線，如圖6所示。6 個舵機角度呈周期性平滑變化，如圖7所示。舵機角度變化頻率與平臺輸入一致，與其它自由度單獨測試結果一致，驗證了平臺結構及參數(shù)設置的合理性。實測表明，該平臺可滿足多自由度同時輸入運行，各自由度角度誤差在±1.5°范圍內(nèi)，平移誤差在±5 mm 范圍內(nèi)。

圖6 x 軸余弦輸入曲線Fig.6 Input cosine curve of X-axis

圖7 x 軸余弦輸入舵機角度變化曲線Fig.7 Curve of servo gear angle with cosine input of X axis

4.2 P3D 聯(lián)機測試

飛行模擬器軟件P3D（Prepar 3D）姿態(tài)數(shù)據(jù)通過共享內(nèi)存的方法進行提取，當兩個進程通過頁表將虛擬地址映射到物理地址時，在物理地址中有一塊共同的內(nèi)存區(qū)，即共享內(nèi)存，可被兩個進程同時讀取使用。模擬器軟件Prepar 3D 及所設計的MFC 接口程序通過共享內(nèi)存的辦法進行數(shù)據(jù)共享，使進程間無需傳送數(shù)據(jù)，而是直接通過內(nèi)存訪問，提高了程序的效率。獲取實時飛行姿態(tài)數(shù)據(jù)后，通過運動學反解先求出驅動桿的伸縮量，進而求解舵機角度，然后向單片機發(fā)送舵機角度控制參數(shù)。測試結果表明，通過共享內(nèi)存方法，飛行姿態(tài)數(shù)據(jù)抓取實時性好、效率高，下發(fā)數(shù)據(jù)實時性好，姿態(tài)仿真平臺在獲取下發(fā)指令后能進行快速響應，并可與P3D 模擬飛行軟件聯(lián)動，進行虛實結合的飛行姿態(tài)仿真。

圖8 模擬飛行軟件與仿真平臺聯(lián)機測試Fig.8 Joint test of flight simulation software and simulation platform

5 結語

結合實際應用需求，設計了以舵機為執(zhí)行機構的桌面型六自由度姿態(tài)仿真平臺，根據(jù)運動學反解理論進行驅動桿伸縮量的解算，進而求解出舵機轉動角度。該仿真平臺體積小、成本低、易于維護，既可以單獨通過上位機軟件運行，也可與P3D 模擬飛行軟件聯(lián)動，性能穩(wěn)定。該平臺實現(xiàn)了對滾轉、俯仰、偏航及3 個方向平移運動的仿真控制。后續(xù)研究將考慮速度及加速度控制，以實現(xiàn)更逼真的模擬器姿態(tài)仿真效果。