六自由度運動平臺運動學建模與仿真分析

劉喜藏， 馬銘澤， 林 皓

(航空工業第一飛機設計研究院，陜西 西安 710089)

飛行模擬器作為重要的地面仿真設備，是典型的人在回路實時仿真系統。六自由度運動平臺作為飛行模擬器的重要組成部分，其根據飛機的飛行狀態進行實時解算并控制平臺進行俯仰、滾轉、偏航、升降、縱向和側向平移運動，從而為飛行員提供瞬態過載感受[1]。六自由運動平臺的結構形式多種多樣，其中并聯結構形式因其具有精度高、剛度大、運動速度快等特點而被廣泛應用。并聯結構六自由度運動平臺的控制目標與作動系統之間是較為復雜的非線性關系，建立運動平臺的運動學關系是進行系統結構設計、運動誤差分析和實現系統精確、快速控制的重要前提條件。

針對并聯結構形式的運動學研究具有逆解容易、正解難的特點，目前已有大量學者開展了相關研究，綜合近年研究成果，可劃分以下3類：數值法、解析法和基于高級算法的求解法[2]。其中，最典型的數值法便是牛頓迭代法，其求解原理簡單易懂，只要迭代次數足夠，就能求得滿足精度要求的方程解，但迭代初值的選擇會影響該方法的迭代速度和尋優結果。陳曉江[3]通過將運動學正解方程表示為9個方程組，采用牛頓迭代法進行了求解。楊小龍[4]使用了一種牛頓迭代法的優化算法，通過構造迭代序列，完成了運動學正解求解，并將求解效率提高到0.3 ms。解析法通常采用消元方式消去約束方程中的未知數，從而獲得方程解。通過解析解可以求得所有解，然而其推導過程十分復雜。Dietmaier[5]證明了一般 6-UPS 機器人有 40 個實數解。對于6-UPS機器人，Sreenivasan等[6]證明最多有 16 個解，當上、下平臺是對稱六邊形時，Huang等[7]證明最多有 14 個解。為解決運動學正解解算問題，胡啟國[8]、弓瑞[9]等采用遺傳算法求得平臺的位姿正解，該算法的解算精度較高，但其尋優解算用時較長。荊學東[10]、朱齊丹[11]等采用神經網絡、牛頓迭代混合的算法完成了Stewart平臺的正解，其迭代效率和解算誤差均優于傳統BP神經網絡算法。

在進行六自由度運動平臺設計時應避免奇異結構的出現，因此其包線內的運動學正解是唯一的。采用遺傳算法、神經網絡等先進算法進行運動學求解時需要進行大量的樣本訓練，很多時候也需要使用牛頓迭代法。六自由度運動平臺的空間包線較小，非常適合采用牛頓迭代法進行運動學正解求解。改進的牛頓迭代法雖然效率較高，然而其構造過程復雜。本文將從簡化運動學正解方程出發，結合六自由度運動平臺的使用特點，通過選擇合理的迭代初值完成位置正解的高效迭代尋優，同時利用解析法完成平臺速度和加速度正解求解。

1 六自由度運動平臺

一個典型的六自由度運動平臺如圖1所示，可以將其劃分為上平臺、下平臺和支腿，其中下平臺固定在地面上，支腿與上、下平臺相交的位置為鉸鏈點，同一平面上的鉸鏈點位于同心圓上。

圖1 六自由度運動平臺結構圖

支腿可以繞上、下鉸鏈進行轉動，并且支腿上部能進行伸縮運動。通過控制6條支腿的伸縮運動來實現對上平臺的控制，從而為人在回路的仿真試驗提供動感效應。

根據上、下平臺圓半徑、鉸鏈間距和支腿長度即可確定平臺的空間結構。平臺鉸鏈點的平面布局如圖2所示，d1～d6為下鉸鏈點，u1～u6為上鉸鏈點，同一平面上的鉸鏈點對稱分布。

圖2 運動平臺鉸鏈點平面分布圖

2 坐標系定義

為了描述運動平臺各節點組成部分在空間的運動學關系，需要建立統一的坐標系。

2.1 下平臺坐標系

下平臺坐標系(FI)坐標原點OI位于下鉸鏈構成大圓的圓心，OIxI軸垂直于下平臺d1、d2號鉸鏈點的連線，OIyI垂直于OIxI指向右，并與OIxI在同一水平面內，如圖2(a)所示。OIzI鉛垂向下指向地心方向。

2.2 上平臺坐標系

當支腿的長度相等且為某特定初始值Lt0時，即Lt1=Lt2=Lt3=Lt4=Lt5=Lt6=Lt0，上平臺與下平臺平行，并定義該位置為運動平臺中立位。規定此時的上平臺圓心Os為上平臺坐標系(Fs)原點，縱軸Osxs垂直于上平臺u4、u5號鉸鏈點的連線，橫軸垂直于Osxs指向右，如圖2(b)所示。豎軸Oszs垂直于Osxsys平面，向下為正。上平臺坐標系是一個可動坐標系，相對于下平臺坐標系，除了坐標原點的平移運動外，還包括俯仰、滾轉和偏航運動，其正方向定義與飛機歐拉角類似。

2.3 支腿坐標系

為了描述支腿向量與上平臺、下平臺之間的關系，需要建立支腿坐標系。下面以下平臺d5鉸鏈點所在的支腿坐標系為例進行說明：支腿慣性坐標系Fa的縱軸Oaxa軸平行于下平臺相鄰鉸鏈點短邊中垂線，正方向指向大圓圓心，橫軸Oaya垂直于Oaxa軸，指向右側，如圖3所示。Oaza軸指向地心方向。

圖3 支腿坐標系定義

3 運動學關系

六自由度運動平臺的上平臺通過支腿與下平臺相連，支腿坐標系相對于支腿慣性坐標系存在轉動。建立支腿與上、下平臺之間的運動學關系，需要利用點的復合運動理論進行分析。

3.1 點的復合運動

以任一支腿為例進行復合運動分析，已知該支腿伸縮速度和伸縮加速度分別為vr1和ar1，其方向沿支腿方向；支腿繞下鉸鏈d1的角速度和角加速度分別為ωt1和αt1，其方向垂直于支腿矢量；支腿在慣性坐標系下的空間矢量為r1。

圖4 支腿復合運動分析

根據速度合成定理，空間任一動點的絕對速度等于其牽連速度與相對速度的矢量和[12],則上鉸鏈點u1相對于下鉸鏈點d1的速度為

va1=ωt1×r1+vr1

(1)

式中:ωt1×r1垂直于支腿方向；vr1沿支腿方向。

支腿上鉸鏈點相對于下鉸鏈的牽連運動為定軸轉動與平移運動之和，根據定軸轉動加速度合成定理，則上鉸鏈點u1相對于下鉸鏈點d1的加速度為

aa1=αt1×r1+ωt1×ωt1×r1+ar1+2ωt1×vr1

(2)

式中:αt1×r1垂直于支腿方向；ωt1×ωt1×r1沿支腿方向，其大小為|ωt1|2·|r1|；ar1沿支腿方向；ωt1×vr1垂直于支腿方向。

3.2 運動學方程

根據平臺結構參數，可以確定鉸鏈點在相對應同心圓上的位置。假設6個下鉸鏈點在FI坐標系下的位置為Pd，6個上鉸鏈點在Fs坐標系下的位置為Pu，Pd和Pu均為3×6的矩陣。

上平臺坐標系原點Os在下平臺坐標系FI下的位置、速度和加速度分別為PI=[x;y;z]，VI=[vxu;vyu;vzu]和AI=[axu;ayu;azu]；上平臺姿態角、角速度和角加速度分別為Euler=[θ;φ;ψ]，ωs=[p;q;r]和αs=[dp;dq;dr]。

6個支腿在FI坐標系下的空間矢量為Pu2d，該矢量為3×6的矩陣。沿支腿方向的伸縮速度和伸縮加速度分別為vr和ar，均為6×1的矩陣。

根據上述定義，建立上平臺位姿、速度和加速度與支腿位置、速度和加速度的運動學對應關系。

(1) 位置運動學關系。

根據上平臺的姿態角，可以得到體軸系-地軸系轉換矩陣Mbtg[13]，該矩陣為3×3方陣。利用該轉換矩陣可以將上平臺上任一點轉換到慣性坐標系。

根據空間矢量定義，可以建立運動平臺位置對應關系：

Pu2d(:,i)=PI+Mbtg·Pu(:,i)-Pd(:,i)，i=1,2,…,6

(3)

(2) 速度運動學關系。

假設上鉸鏈點在FI坐標系下的速度為Vu，該矢量為3×6的矩陣，則

Vu(:,i)=VI+ωs×Pu(:,i)，i=1,2,…,6

(4)

結合式(1)中點的復合速度分析，Vu沿支腿方向的映射速度即為支腿的伸縮速度：

|vri|=Vu(:,i)·Pu2d(:,i)/|Pu2d(:,i)|，i=1,2,…,6

(5)

(3) 加速度運動學關系。

假設上鉸鏈點在FI坐標系下的加速度為Au，該矢量為3×6的矩陣，則

Au(:,i)=AI+ωs×ωs×Pu(:,i)+αs×Pu(:,i)，i=1,2,…,6

(6)

上鉸鏈點慣性系下的速度Vu沿垂直支腿方向的映射分量是由支腿的轉動產生的。假設支腿的角速度為ωt，ωt為3×6矩陣，結合式(1)可求得：

ωt(:,i)=Vu(:,i)×Pu2d(:,i)/|Pu2d(:,i)|，i=1,2,…,6

(7)

再結合式(2)，可求得支腿的伸縮加速度：

|ari|=Au(:,i)·Pu2d(:,i)/|Pu2d(:,i)|-|Pu2d(:,i)|·|ωt(:,i)|2，

i=1,2,…,6

(8)

至此，完成了上平臺與支腿之間的運動學關系映射。

4 運動學正、逆解

六自由度運動平臺的運動學正解是指根據支腿的位置、速度和加速度確定上平臺的位姿、速度和加速度的過程；而運動學逆解則是由上平臺的位姿、速度和加速度求解支腿的位置、速度和加速度的過程。可以將六自由度運動平臺看作是并聯結構機器人，其運動學方程具有逆解容易、正解難的特點。

4.1 運動學逆解

已知上平臺的位置、姿態、速度、加速度、角速度和角加速度，基于式(3)、式(5)和式(8)，可以直接求得支腿的位置、伸縮速度和伸縮加速度，即完成運動學逆解，這里不再贅述。

4.2 運動學正解

4.2.1 位置正解

位置正解需要解決的問題可以描述為：已知運動系統的結構參數，給定6個作動筒的長度Lt(i)，i=1,2,…，6，求解上平臺坐標系原點位置PI=[x;y;z]和姿態Euler=[θ;φ;ψ]。對運動系統進行位姿正解是一個求解高階非線性方程組的過程[3]。

利用式(3)，可得到以下方程組：

fi(x,y,z,θ,φ,ψ)=|PI+Mbtg·Pu(:,i)-Pd(:,i)|-Lt(i)=0，
i=1,2,…,6

(9)

則有一階泰勒展開式

可用矩陣表示為

0≈f(x(k))+J(x(k))(a-x(k))

解得：

a≈x(k)-J(x(k))-1f(x(k))

于是，就得到了牛頓迭代格式：

x(k+1)≈x(k)-J(x(k))-1f(x(k))

式中:J為雅可比矩陣。

當f(x(k+1))滿足設定的誤差要求時，則認為此時的x(k+1)為位置正解，即平臺的位置和姿態。

4.2.2 速度正解

速度正解需要解決的問題可以描述為：已知運動系統的結構參數，給定6個作動筒的長度Lt(i)、伸縮速度|vri|，i=1,2,…,6，求解上平臺速度VI=[vxu;vyu;vzu]。

利用式(4)和式(5)，得到下面方程組：

gi(vxu,vyu,vzu)=(VI+ωs×Pu(:,i))·Pu2d(:,i)/
|Pu2d(:,i)|-|vri|=0，i=1,2,…,6

(10)

式(10)包含6個方程式，利用廣義擬求解方法可以求得上平臺的速度VI=[vxu;vyu;vzu]。

4.2.3 加速度正解

加速度正解需要解決的問題可以描述為：已知運動系統的結構參數，給定6個作動筒的長度Lt(i)、伸縮速度|vri|、伸縮加速度|ari|，i=1,2,…,6，求解上平臺加速度AI=[axu;ayu;azu]。

利用式(6)～式(10)，得到下面方程組：

hi(axu,ayu,azu)=Au(:,i)·Pu2d(:,i)/|Pu2d(:,i)|-
|Pu2d(:,i)|·|ωt(:,i)|2-|ari|=0，i=1,2,…,6

(11)

式(11)包含6個方程式，利用廣義擬求解方法可以求得上平臺加速度AI=[axu;ayu;azu]。

4.3 運動學解算流程圖

六自由度運動平臺的運動學解算過程如圖5所示。根據平臺的結構參數和上平臺目標運動參數，經運動學逆解得到支腿的位置、速度和加速度；利用運動學正解方程完成平臺位姿、速度和加速度的正解。

圖5 六自由度運動平臺運動學解算流程圖

5 仿真結果與分析

以某運動平臺結構參數為例，對六自由度運動平臺的運動學關系進行仿真建模和驗證。該運動平臺結構參數如表1所示。

表1 某運動平臺結構參數列表 單位：mm

仿真模型結構如圖6所示。模型由運動學逆解模塊、作動系統、運動學正解模塊3個部分組成，模塊之間的關系如下。

圖6 六自由度運動平臺運動學模型結構圖

① 運動學逆解模塊的輸入為平臺的目標位置、速度、加速度、歐拉角、角速度和角加速度指令，經運動學逆解解算后，得到支腿的作動指令。

② 作動系統根據作動指令驅動支腿運動，并可以通過電機編碼器得到支腿的實際位置、速度和加速度。

③ 將支腿運動狀態輸入運動學正解模塊，解算得到平臺的實際運動狀態。

為了驗證平臺的運動學關系，這里假設作動系統為無延遲的理想環節，則平臺目標運動指令值應與運動學正解模塊解算得到的平臺狀態一致。

5.1 模型解算性能分析

運動學模型中的運動學逆解模塊可以看作線性定常系統，其解算速度很快。又因為速度和加速度正解方程也是線性的，因此重點對運動學位姿正解解算過程進行分析。

選擇3個不同的支腿長度狀態作為運動學正解模塊輸入，以平臺位于中立位作為迭代尋優初值，尋優過程如表2所示。其中狀態點1=[4795;4795;4795;3295;3295;3295]，狀態點2=[3300;3581;4158;4521;3854;4029]，狀態點3= [3500;4795;4500;3600;3800;4300]。

表2 運動學正解迭代尋優過程

由表2可知，對于不同的支腿狀態，經5次迭代尋優后，即可得到上平臺的位姿參數。尋優耗時不超過2 ms，均方差小于1，完全滿足工程應用實時性要求。

將迭代過程的均方差用圖形表示出來，如圖7所示，可以更好地展示尋優收斂速度。事實上，經過4次迭代后的解算精度已經很高。

圖7 尋優結果均方差隨迭代次數增加變化圖

采用牛頓迭代法進行位姿正解求解，不同的迭代初值會影響模型的解算效率。下面將通過選擇不同的迭代初值對迭代結果和效率進行分析。

假定迭代初值為上平臺圓心在下平臺坐標系下的位姿，尋優初值1=x(0)=(0,0,3000,0°,0°,0°)位于下平臺以下，即不在運動平臺包線范圍內。尋優初值2=x(0)=(0,0,-6000,0°,0°,0°)位于下平臺之上，同樣超過系統的包線范圍。尋優初值3=x(0)=(0,0,-3147,0°,0°,0°)是平臺位于中立位時的上平臺位姿參數。針對狀態點1～狀態點3，采用不同的迭代初值進行解算，尋優結果如表3所示。

表3 不同迭代初值的尋優結果

由表3可知，不合理的初值設定(例如初值1)，會導致尋優結果不在運動系統包線范圍內；而將初值設定在下平臺之上的兩個狀態點，尋優結果合理且唯一，初值與尋優結果較近的狀態點尋優時間略短，總體尋優時間均不超過2 ms。

考慮飛行模擬器六自由度運動平臺的工作特點：平臺每次均從中立位開始運動，在洗出算法影響下選擇“初值3”，即平臺中立位的位姿作為迭代初值，原因如下：① 運動平臺的初始運動均從中立位開始；② 每完成一次突發運動，在洗出算法的影響下平臺會“悄悄”回到中立位[14-15]。因此，選擇中立位的位姿作為迭代初值可以最大限度地減少尋優時間。

5.2 運動包線解算

在進行運動平臺包線計算時，將6個支腿由最短到最長進行遍歷，由此求得上平臺的最大空間狀態即為平臺的運動包線。以單個支腿取100個特征點為例，則完成支腿的遍歷需要進行1006次計算。假設單次計算耗時1 ms，完成包線計算時間需要超過26萬小時。

筆者提出一種計算運動平臺包線的可行方案，即每個支腿只選擇最長、最短2個狀態進行遍歷，經過26=64次計算，即可完成運動包線解算。

表4中的L為0代表最短支腿長度，1代表最長支腿長度。完成表4中運動系統的包線解算僅用時55 ms。由計算結果可知，本文建模所用的六自由度運動平臺縱向運動范圍為±1.5 m，側向運動范圍為±1.6 m，垂向運動范圍為±0.9 m。平臺的空間包線是其重要性能指標，一定程度上決定了平臺的運動能力。

表4 六自由度運動平臺空間包線

5.3 復合運動分析

選取一組正弦波復合運動數據作為上平臺指令值輸入，經運動學逆解模塊解算后，得到6個支腿的作動指令，經作動系統后驅動上平臺運動。將上平臺的指令值與運動學正解模塊輸出值進行對比，如圖8～圖10所示。

圖8 平臺位置指令與響應值對比

由圖9和圖10可知，平臺的指令值與響應值完全一致，證明了所建立的六自由度運動平臺運動學關系正確。當然，對于實際作動系統，平臺指令值與響應值之間將存在一定的延遲與偏差。

圖9 平臺速度指令與響應值對比

圖10 平臺加速度指令與響應值對比

6 結論

通過引入點的復合運動理論，建立了平臺與支腿之間的位置、速度、加速度運動學關系。利用MATLAB/Simulink環境搭建了運動學模型，并設計了合理的驗證方法。仿真結果表明,所建立的運動學模型具有解算速度快、準確度高等特點，可滿足工程實時解算需求。本文的創新點和貢獻如下：

① 針對六自由度運動平臺的非奇異結構特點，選擇牛頓迭代法完成運動學位置正解求解；提出了一種合理的初值選擇方法，并證明了合理區域內即使初值與尋優結果差異較大，也可以完成算法的快速收斂。

② 分析指出采用遍歷的方法求得運動系統的包線非常困難，給出了一種可行的運動包線尋找方法，通過6個支腿的狀態組合完成了運動平臺空間包線的計算。

③ 在完成位置正解的基礎上，創新性地引入點的復合運動理論，利用解析的方法完成了六自由度運動系統速度和加速度正解，為作動系統伺服電機控制引入了更多的控制參數，有助于提高控制精度。