Stewart六自由度并聯機構位姿分辨率分析方法

梁鳳超，趙筱琳，譚爽，范建凱，林喆，石志成

（北京空間機電研究所，北京 100090）

近年來，空間光學遙感器分辨率需求越來越高，使得超長焦距、超大口徑空間光學遙感器成為重要研究方向之一。受鏡面加工能力及火箭運載能力的限制，超大口徑空間遙感器主鏡通常采取分塊拼接的方式[1-2]。而在軌展開機構拼接主鏡精度有限，需高分辨率位姿調整機構對分塊鏡群及次鏡的位姿實施在軌校正，以滿足超大口徑空間望遠鏡高分辨率成像的要求。即使對于主鏡未采取分塊拼接方式的光學遙感器，相機入軌后力熱環境變化也會導致主次鏡空間位姿發生改變，同樣需要高分辨率位姿調整機構在軌調整次鏡位姿[3-4]。而完成主鏡分塊鏡及次鏡位姿精密調整的關鍵是六自由度并聯機構，該機構的位姿分辨率是影響分塊鏡及次鏡位姿調整的關鍵。空間光學遙感器地面光學裝調及光學像差在軌主動校正亟需研制高分辨率六自由度并聯機構。

研制高分辨率六自由度并聯機構不能也不必在其位姿分辨率參數還不明確的情況下制造實體機構，因為這是成本極高且很難收到良好效果的。因此，需要一種分析方法，實現在設計階段分析六自由度并聯機構位姿分辨率，確保其關鍵性能滿足使用要求。針對此需求，本文提出一種基于驅動支鏈實際分辨率的位姿分辨率分析方法，為研制高分辨率Stewart六自由度并聯機構提供有力的分析保障。

1 運動學建模

典型的Stewart構型6-UCU（U-虎克鉸副，C-圓柱副）六自由度并聯機構組成及坐標系定義如圖1、圖2所示。六自由度并聯機構由上下兩個平臺和6個并聯的、可獨立自由伸縮的驅動支鏈組成，驅動支鏈和平臺之間通過虎克鉸A1、A2、A3、A4、A5、A6和虎克鉸B1、B2、B3、B4、B5、B6連接。將下平臺作為基礎平臺（定平臺），以驅動支鏈的伸縮位移作為輸入變量，則可以控制上平臺（動平臺）的空間位移和姿態。如圖2所示，六自由度并聯機構的結構參數有r-動平臺鉸點圓半徑，R-靜平臺鉸點圓半徑、α-動平臺鉸點短邊圓心角一半、β-靜平臺鉸點短邊圓心角一半和H-并聯機構中位時p與O的距離，確定上述五個參數，就可以完全描述Stewart六自由度并聯機構的基本結構。

圖1 典型Stewart

圖2 上下平臺鉸點分布

1.1 運動學逆解數學模型

如圖1、圖2所示，為了建立Stewart六自由度并聯機構運動學數學模型[5-10]，在靜平臺鉸點所在圓的圓心處建立靜坐標系O-XYZ，坐標原點為O；在動平臺鉸點所在圓的圓心處建立動坐標系p-xyz，坐標原點為p。坐標系確定之后，六自由度并聯機構的運動平臺的位姿用廣義坐標向量q表示，其中q=[x y z U V W]T，[x y z]T表示p作為旋轉中心時，點p在靜坐標系中的坐標向量，[U V W]T表示p-xyz在靜坐標系中的姿態角。這六個參數就確定了動平臺的空間位姿。旋轉變換矩陣可將動平臺各點坐標統一在靜坐標系中，根據動平臺目標位姿計算出驅動支鏈位置（運動學逆解）或根據支鏈位置求出當前動平臺位姿（運動學正解）。旋轉變換矩陣表達式如下[11-13]：

其中，cU=cos（U），cV=cos（V），cW=cos（W）；sU=sin（U）；sV=sin（V）；sW=sin（W）。動平臺姿態廣義坐標向量q=[x y z U V W]T時，各驅動支鏈的長度如下：

其中，矢量Li為圖1所示的支鏈矢量AiBi；li為支鏈長度，i=1，2，…，6，i表示驅動支鏈的序號。至此，建立了并聯機構運動學逆解數學模型。

1.2 運動學正解數學模型

目前并聯機構的正解主要有數值法[14-15]和解析法[16-17]。數值法就是給定一個初值，從這個初值開始通過循環、迭代直到獲得滿足給定精度要求的解。數值法數學模型簡單，可以求解任何并聯機構。下面應用Newton-Raphson法建立六自由度并聯機構運動學正解數值解模型。定義下面的目標向量函數f來描述驅動支鏈伸縮長度的估計值li與實測值|Li|之間的不同，即：

式中，li表示估計位姿對應的6個驅動支鏈伸縮長度的估計值；|Li|表示6個驅動支鏈伸縮長度的實測值；f1…f6分別表示li的平方與實測值|Li|的平方之間的偏差。

Newton-Raphson法以目標向量函數f取極小值為目標，求解六自由度并聯機構位姿數組Q的步驟如下：

（1）測量6個驅動支鏈的坐標向量Li，計算驅動支鏈伸縮長度的實測值|Li|；

（2）選取運動平臺的位姿初值Q，該位姿初值Q，利用逆解公式計算各支鏈伸縮長度估計值li；

（3）基于6個驅動支鏈伸縮長度的實測值和估計值li，構成數組函數f；

（4）計算雅克比矩陣；

（5）利用JδQ=-f計算位姿修正值δQ；

（6）如果δQTδQ＜ε（ε為位姿誤差閾值）成立，則Q為所求的位姿，否則，進行下一步；

（7）計算Q=Q+δQ，并轉到步驟（2）。

步驟（4）中雅克比矩陣的計算公式如下：

Li=[lixliyliz]T為第i個驅動支鏈的坐標向量；為動平臺上第i個驅動支鏈對應的鉸點在動坐標系p-xyz坐標系下的坐標向量；Rcol2、Rcol3分別表示旋轉變換矩陣R的第2、3 列；Rrow1、Rrow2、Rrow3分別表示R的第 1、2、3行。

2 位姿分辨率分析方法

位移分辨率是給定指令后機構的最小運動增量，包括設計分辨率與實際分辨率。例如脈沖當量即為設計分辨率。但在實際應用中，由于間隙、形變等因素的影響，機構很難具有脈沖當量那么小的運動增量，實際運動分辨率會是脈沖當量數倍甚至數十倍，且只能由實測得到。

2.1 位姿分辨率

六自由度并聯機構位姿分辨率是六個單自由度的最小運動增量[18]：即3個平移分辨率Δx、Δy、Δz和 3個繞x、y、z軸的轉角分辨率 ΔU、ΔV、ΔW。

常規笛卡爾坐標運動機構每個驅動軸對應一個自由度，控制方式是獨立控制每個驅動支鏈來控制該自由度的運動。雖然六自由度并聯機構位姿分辨率指標是以笛卡兒坐標系形式表達的，但是六自由度并聯機構是非線性、強耦合的多輸入多輸出系統：并聯機構的一個輸入即任一驅動支鏈伸縮，會影響所有輸出，任一自由度輸出由所有輸入即驅動支鏈伸縮量來決定。想要得到單自由度運動分辨率，必須通過計算得到特定的控制指令，控制六個驅動支鏈運動，實現僅在指定的單自由度輸出最小運動增量，而其他五個自由度輸出為零或者可忽略的耦合運動量。該指定自由度的最小運動增量即為該自由度的運動分辨率。可見，分析六自由度并聯機構位姿分辨率的關鍵之一就是求得動平臺的單自由度運動指令，即求得一組驅動支鏈增量數組，按該數組驅動六支鏈即可實現動平臺的單自由度最小運動。

另外，在分析六自由度并聯機構的分辨率時，不能以支鏈理論計算脈沖當量去分析，而需考慮支鏈的實際分辨率，這樣得到的分辨率更接近工程實際。

2.2 位姿分辨率分析方法

六自由度并聯機構位姿分辨率分析方法如圖3所示，以運動逆解、正解數學模型為基礎、利用支鏈增量縮放不改變單自由度運動的特性、以驅動支鏈實際分辨率為量化基準，通過目標位姿計算目標支鏈增量，再對目標支鏈增量量化、尋找最小量化倍數絕對值，得到最小支鏈增量數組、支鏈長度，進而應用正解數學模型求出六自由度并聯機構位姿分辨率。該分析方法具體步驟如下：

圖3 六自由度并聯機構位姿分辨率分析方法

（1）建立運動學模型

建立公式（1）～公式（4）所示的六自由度并聯機構的運動學逆解、正解模型。

（2）初始化參數

初始化如下參數：結構參數r、R、α、β、H，支鏈實際分辨率FBL，目標位姿系數n（n一般取10～20），初始位姿q1=[x1y1z1U1V1W1]T，自由度序號j。

（3）計算q1對應的支鏈長度數組L1

調用運動學逆解模型，計算q1對應的支鏈長度L1=[l1l2l3l4l5l6]T。

（4）給定目標位姿

（5）計算支鏈長度增量數組

（6）ΔL按支鏈分辨率量化。

量化的意義在于得到各個支鏈的實際可達運動增量。ΔL按支鏈實際分辨率量化得到量化支鏈增量數組qΔL：

qΔL=[qΔl1qΔl2qΔl3qΔl4qΔl5qΔl6]T，量化的算法為：qΔli=Mi×FBL，Mi=int（Δli/FBL）。Mi為 ΔL相對于支鏈分辨率的量化倍數，int（）表示取整運算。

此時，將qΔL作為增量施加到各支鏈時，并聯機構會輸出步驟（4）中給定的單自由度運動，但因qΔL不是最小支鏈增量數組，該單自由度運動增量不是分辨率。由于對于同一個結構參數的并聯機構，在對其進行單自由度驅動時，支鏈長度增量數組同時放大或縮小正數倍（僅縮放、不改變方向）時，機構仍保持原指令的單自由度運動。下一步要求出這個最小的正數倍Mmin，將qΔL數值除以Mmin，就得到該自由度分辨率指令，即最小驅動增量數組qminΔL。

（7）計算最小倍數絕對值Mmin

Mmin是計算單自由度分辨率指令即最小驅動增量數組的關鍵。如果支鏈理論增量Δli＜FBL，則量化倍數Mi=int（Δli/FBL）=0，進而量化值qΔli=Mi×FBL=0。確定Mmin要去掉為 0 的Mi，得到新的一組Mk∈Mi，Mk≠ 0，k=1，2，…，n，k≤6；然后，找出非零Mk絕對值的最小值Mmin：Mmin=Min（abs（Mk））；其中，abs（Mk）得到量化倍數的絕對值，abs（）是取絕對值運算；Min（abs（Mk））得到Mk的最小值絕對值，Min（）為取最小值運算。

（8）計算單自由度分辨率指令

將qΔL=[qΔl1qΔl2qΔl3qΔl4qΔl5qΔl6]T除以Mmin，即可得到該自由度分辨率指令：最小驅動增量數組qminΔL。

其中，qminΔli=int（qΔli/Mmin）×FBL。

（9）計算分辨率位姿q2

單自由度分辨率指令的支鏈長度數組為：

L2=L1+qminΔL=[l1+qminΔl1l2+qminΔl2l3+qminΔl3l4+qminΔl4l5+qminΔl5l6+qminΔl6]T。 然后 調用 正解數學模型，計算L2對應的位姿q2：q2=[x2y2z2U2V2W2]T。

（10）計算單自由度分辨率

分辨率位姿q2與初始位姿q1相減，得到該自由度分辨率：

當j=1時，得到x自由度分辨率Δx，Δqx=q2-q1；同理，j=2～6 時，分別得到 Δy、Δz、ΔU、ΔV、ΔW。

可根據需要，從步驟（2）開始，重新輸入六自由度并聯機構的結構參數與支鏈分辨率，進行新一輪分辨率分析。

3 分辨率分析及測試

3.1 分辨率分析

某空間相機次鏡口徑0.5 m，重25 kg，需研制高分辨率次鏡Stewart平臺，實現相機地面光學裝調及在軌像差主動校正模擬。該Stewart平臺分辨率指標為Δx、Δy、Δz：0.2 μm，ΔU、ΔV、ΔW：1″。

經過六自由度并聯機構的參數化建模、運動學動力學分析、結構優化設計，得到六自由度并聯機構的結構參數：r=239.51 mm、R=249 mm、α=6.589°、β=7.677 6°和H=195.03 mm。然后，須根據平臺分辨率指標計算驅動支鏈實際分辨率需求。根據平移分辨率0.2μm、旋轉分辨率1″的指標要求，在平臺分別施加單自由度分辨率的運動增量，進行運動學逆解運算，可得到平臺分辨率對驅動支鏈的分辨率需求。分析結果顯示需要驅動支鏈分辨率優于60 nm才能實現平臺的分辨率指標。該平臺采用的驅動支鏈及其分辨率如圖4所示，其理論分辨率即脈沖當量為5 nm，實際分辨率為50 nm，滿足驅動支鏈優于60 nm的需求。

圖4 驅動支鏈位移分辨率達到50 nm

接下來，須基于結構參數及驅動支鏈實際分辨率，分析Stewart平臺的分辨率。由于六個自由度分辨率分析過程類似，下面僅以平移x方向分辨率為例，介紹一下分析過程。

首先，根據結構參數，計算上下鉸點Ai、Bi的空間坐標、旋轉矩陣R，得到運動學數學模型。由于初始位姿的選取不影響分析結果，取初始位姿q1為零位位姿。然后，調用運動學逆解模塊，計算q1對應的桿長數組：L1=[272.301 812 272.301 812 272.301 812 272.301 812 272.301 812]T。之后，取n=10，則x方向目標位移增量為x=n×FBL=10×50 nm=0.000 5 mm。給定目標位姿qx=[0.000 5 0 0 0 0 0]T。接著調用運動學逆解模塊，計算得到qx對應的桿長數組。L2'=[272.301 862 272.301 712 272.301 862 272.301 862 272.301 712 272.301 862]T。則支鏈長度增量數組ΔL=L2'-L1=[0.000 157-0.000 348 0.000 191 0.000 191 -0.000 348 0.000 157]T。之后，ΔL按支鏈分辨率量化后得到qΔL=[0.000 15-0.000 3 0.000 15 0.000 15 -0.000 3 0.000 15]T，但因qΔL不是最小支鏈增量數組，由qΔL作為輸入得到的x方向運動增量不是分辨率。必須計算qΔL的最小倍數絕對值，進行等比例縮小，得到最小支鏈增量數組、支鏈長度數組。

經計算得到最小倍數絕對值Mmin=3。將qΔL等比例縮小3倍即可得到x自由度分辨率指令。qminΔL=[0.000 05 -0.000 1 0.000 05 0.000 05-0.000 1 0.000 05]T。此即分辨率位姿q2的驅動支鏈增量數組。而此時q2對應的單自由度分辨率指令的支鏈長度數組為：L2=L1+qΔL=[272.301 812 272.301 812 272.301 812 272.301 812 272.301 812]T。此時，調用正解數學模型，計算L2對應的分辨率位姿q2：q2=[0.166 7 0 0 0 -0.000 112 0]T。而 Δqx=q2-q1=[0.166 7 0 0 0 -0.000 112 0]T，則x自由度的分辨率為Δx=0.166 7 mm，繞x方向上有-0.000 112″的耦合量，與1″的指標相比可以忽略。

應用基于VC++開發了六自由度并聯機構位姿分辨率分析軟件，分辨率分析算法及工作流程如圖3所示，軟件界面如圖5所示。經計算，得到該Stewart平臺的分辨率分別為：Δx=0.17μm，Δy=0.15 μm，Δz=0.14 μm，ΔU=0.77″、ΔV=0.74″、ΔW=0.75″。部分中間結果如“支鏈增量（μm）”“增量量化（μm）”“最小驅動量（mm）”會顯示出來，最后會顯示分析結果：分辨率（μm/″）。理論分析結果表明：該Stewart平臺平移分辨率優于0.2 μm，轉角分辨率優于1″。

圖5 位姿分辨率分析軟件

3.2 分辨率測試

研制的Stewart次鏡并聯機構如圖6所示。該并聯機構的位移及轉角分辨率是實現次鏡位姿精密調整的關鍵，這就需要建立六自由度位姿測量系統，對該并聯機構的位姿調整分辨率及調整范圍進行測試。

圖6 Stewart并聯平臺

Stewart次鏡并聯機構平移及轉角分辨率測試系統如圖7所示。系統由RENISHAW XL-80雙頻激光干涉儀、AUTOMAT自準直儀、Stewart次鏡平臺及模擬負載、并聯機構控制器組成。雙頻激光干涉儀用于平移分辨率測試，自準直儀用于轉角分辨率測試。整個測試系統搭建在恒溫實驗室內隔振光學平臺上，可有效防止溫漂引起的形變誤差、隔離和衰減低頻、高頻振動。該測試系統平移測量分辨率達到1 nm、角度測量分辨率達到0.002″。

圖7 Stewart并聯機構分辨率測試系統

平臺分辨率測試采用六個自由度分開測試的方法，每個自由度都是先以技術要求指標5倍的步距步進，停留約20 s，使設備穩定下來，再步進，前進5步后，再后退5步。然后依此步驟逐步降低指令值，例如x方向分辨率測試，分別x平移1μm、0.8μm、0.6μm、0.4μm、0.2μm、0.1μm的平移指令，激光干涉儀測試曲線能夠很好分辨出x平移0.2～1μm的指令，而繼續嘗試給出更小的平移指令0.1μm時，測得位移曲線臺階不明顯，故判定平臺的x平移自由度分辨率為0.2μm。x平移自由度實測分辨率如圖8所示。同理測得平臺y、z自由度的平移分辨率也為0.2μm。

圖8 平移x自由度分辨率測試曲線

平臺繞x軸旋轉分辨率指標為1″，依次給定5″、4″、3″、2″、1″、0.5″的轉角步距指令，進行分辨率測試，自準直儀測試結果表明Stewart平臺很好地分辨出了 5″、4″、3″、2″、1″的轉角指令；當指令值低于1″時，曲線臺階不明顯，判定平臺繞x軸轉角分辨率為1″。繞x軸旋轉U自由度分辨率測試曲線如圖9所示。同理測得平臺繞y軸、繞z軸旋轉的分辨率均達到1″。

圖9 繞x軸旋轉U自由度分辨率測試曲線

該Stewart平臺分辨率分析結果為，Δx=0.17μm，Δy=0.15 μm，Δz=0.14 μm，ΔU=0.77″，ΔV=0.74″、ΔW=0.75″。實測結果為平移 0.2 μm，旋轉 1″，與理論分析結果基本一致。

4 結論

高分辨率Stewart六自由度并聯機構是精密調整主鏡分塊鏡及次鏡位姿、實現空間光學遙感器光學像差主動校正的關鍵。為給高位姿分辨率Stewart六自由度并聯機構研制提供分析保障，本文提出了一種在設計階段分析并聯機構位姿分辨率的分析方法。該方法通過建立六自由度并聯機構運動模型，利用支鏈增量縮放不改變單自由度運動的特性，經對支鏈增量進行特殊量化處理，得到最小支鏈增量數組，進而求出位姿分辨率。

基于VC++開發了六自由度并聯機構位姿分析軟件，實現了不同結構參數六自由度并聯機構的位姿分辨率分析。應用該軟件分析了某空間相機次鏡六自由度并聯機構的位姿分辨率，測試結果表明，分析結果與實測結果基本一致。本分析方法成功設計了多臺高分辨率六自由度并聯機構，具有較強的工程指導價值。