基于無量綱化辨識雅可比矩陣選取測量位姿的Stewart并聯機構運動學標定

強紅賓，薛大鵬，馮新宇，張立杰，2*

(1. 燕山大學 河北省重型機械流體動力傳輸與控制重點實驗室，河北 秦皇島 066004；2. 燕山大學 先進鍛壓成形技術與科學教育部重點實驗室，河北 秦皇島 066004)

1 引 言

并聯機構具有結構緊湊、承載能力強、運動精度高、慣性低等優點，因此被廣泛應用于調姿機構[1]、指向機構[2]、加工制造設備等領域。但由于并聯機構制造及裝配過程中產生的誤差不可避免，導致并聯機構的理論運動學參數與實際運動學參數之間也存在一定的誤差，從而使并聯機構的運動偏離理想軌跡，降低精度。通過運動學標定對運動學模型進行改進，可以提高并聯機構的運動精度[3]。

運動學標定通常包括誤差模型、位姿測量、參數辨識和誤差補償四個環節[4]。誤差模型的研究主要包括誤差參數的選擇和基于機構運動學、智能算法等模型的建立等。位姿測量主要包括測量工具的選擇和測量位姿的選擇等。參數辨識主要研究基于最小二乘法、牛頓迭代算法及智能算法的誤差參數求解。誤差補償包括對運動學模型的補償和對控制系統的補償。Gao等[5]建立了Stewart平臺基于運動學模型的誤差模型，并采用激光跟蹤器LTD5OO進行位姿測量。Li等[6]采用雙球棒(DBB)和三軸千分尺測量了三棱錐機器人末端執行器的位置，采用牛頓拉弗森迭代法和最小二乘法辨識出結構參數。Wu等[7]先采用牛頓迭代對并聯機床進行初始誤差參數辨識，再采用遺傳算法進行精細誤差參數辨識，得到了較高精度。

選擇合理的測量位姿可以提高運動學標定對測量噪音的魯棒性。測量位姿的選擇主要包括工作空間內隨機選擇、工作空間內均布選擇、基于正交理論的選擇等方法。文獻[8-9]在Stewart并聯機構工作空間內隨機選取了多個測量位姿。吳江寧等[10]根據Stewart并聯機構關節空間選取了7個測量位姿。Bai等[11]在Delta并聯機構工作空間的邊界選擇多個等距的姿態。GUO等[12]基于正交理論選擇6PUS并聯機械手的測量姿態。Sun等[13]基于三自由度旋轉并聯機器人運動軌跡選擇測量位姿。

為了進一步提高標定對測量噪音的魯棒性，許多學者研究了基于辨識雅克比矩陣的可觀測性指標最大化的測量配置選擇方法。文獻[14-19]提出了5個基于辨識雅克比矩陣的可觀測性指標O1～O5。Daney等[20]采用局部收斂法和禁忌搜索法獲得了一組可觀測性指標最大的測量位姿，并通過Stewart機構的仿真驗證了此測量位姿對傳感器測量噪聲的魯棒性有明顯提升。Zhang等[21]利用DETMAX算法和禁忌搜索算法，提出了一種高效的測量位姿選擇算法，并在5個可觀測性指標下的仿真驗證了該算法的有效性，結果表明通過所選測量位姿可以顯著提高標定方法的魯棒性。Huang等[22]以可觀測性指標O2最大化選取四自由度并聯機器人的測量位姿，通過標定保證并聯機器人的定位和旋轉精度。Gao等[23]以可觀測性指標O4最大化選取六自由度冗余驅動的并聯機器人的測量位姿。Saputra等[24]對Stewart平臺利用群智能搜索算法分別以前4個可觀測性指標最大化搜索了測量位姿，并采用千分尺測量位姿完成了運動學標定。Joubair等[25-26]研究了機器人標定中5項可觀測性指標的有效性，結果表明不同可觀測性指標對不同類型的機器人標定性能不同，整體來說可觀測性指標O1的性能更好一些。

采用與辨識雅可比矩陣相關的可觀測性指標最大化的方法選擇測量位姿，可以提高并聯機構運動學標定對傳感器測量噪聲的魯棒性。但是，辨識雅可比矩陣是并聯機構位移和姿態的函數，且位移和姿態對應的辨識雅可比矩陣中的元素的數量級不同。直接求解辨識雅可比矩陣求取性能指標選擇出的位姿，其位置和姿態對測量噪音的魯棒性不同。因此，本文提出先對辨識雅可比矩陣拆分為位置誤差雅克比矩陣和姿態誤差雅克比矩陣，在通過特征長度使位移和姿態對應的辨識雅可比矩陣中的元素的數量級相同，再通過可觀測性指標選擇測量位姿，保證位移和姿態對測量噪音具有相同的魯棒性。提高機構的標定整體性能和精度。

2 Stewart并聯機構分析

圖1為Stewart并聯機構結構簡圖。該機構的基本構件為靜平臺、動平臺和六個支腿。靜平臺固定，六個支腿為驅動器，通過胡克鉸連接靜平臺與動平臺，動平臺為運動末端。支腿與靜平臺連接的鉸點為Ai(i=1，2，…，6)，與動平臺連接的鉸點為Bi(i=1，2，…，6)。動、靜平臺的胡克鉸分布如圖2所示。

鉸點Ai，Bi的坐標ai，bi的表達式為：

ai=[RacosηiRasinηi0]，

(1)

bi=[RbcosφiRbsinφi0]，

(2)

圖1 Stewart并聯機構結構簡圖Fig.1 Structure diagram of Stewart parallel manipulator

式中：

2.1 逆運動學分析

逆運動學為已知動平臺的位移和姿態求解六個支腿的長度。支腿長度的求解公式為：

li=Rbi+P-ai，

(3)

其中：P=[x，y，z]是動平臺的位移。R為動平臺的旋轉矩陣，本文采用RPY角γ，β，α來表示，具體表達式為：

(4)

其中γ，β，α分別為動平臺繞x軸，y軸，z軸的轉動。

2.2 正運動學分析

正運動學為已知6個支腿的長度求解動平臺的位移和姿態。正解公式為：

f(T)=‖(Rbi+p-ai)‖2-‖li‖2=0.

(5)

并聯機構的位置反解可以通過公式(3)直接求解，而位置正解公式(5)由于包含非線性方程組卻相當復雜。目前常用數值方法對公式(5)進行求解。

3 誤差模型

對于Stewart并聯機構，每個分支有7 個誤差參數，包括上、下平臺的鉸點沿x，y，z3個方向的定位誤差和驅動桿的軸向誤差，則6個分支共有42項誤差。則基于運動學的誤差模型建立對公式(3)兩邊微分：

dliui+lidui=d(R)bi+Rdbi+dp-dai.

(6)

JTdT=Jede，

(7)

其中：

dT=[dxdydzdαdβdγ]T∈R6×1，

de=[dl1da1db1… dl6da6db6]T∈R42×1.

由于在六自由度并聯機構工作空間內，JT為非奇異矩陣，則：

dT=Jde，

(8)

公式(8)即為單位姿運動學誤差模型，1個位姿可得到6個方程，理論上講只需測量7個位姿，即能構造出42 個約束方程，辨識出機構的42個誤差參數。實際標定時由于測量噪音等的存在，需要測量m(m>7)個位姿，以便于更好地辨識誤差參數。則m個位姿的誤差模型為：

dTz=Jzde，

(9)

其中：Tz∈R6m×1，Jz∈R6m×42。

4 基于無量綱化辨識雅克比矩陣的

可觀測性指標

為了提高測量位姿對測量噪音的魯棒性，我們希望誤差參數de的一個非常小的變化能夠對位姿誤差dT產生盡可能大的影響，即‖dT‖/‖de‖盡可能大。

Nahvi等[18]利用辨識雅克比矩陣Jz的奇異值來表‖dT‖/‖de‖：

(10)

其中σL和σ1分別是辨識雅克比矩陣Jz的非零最小奇異值和最大奇異值。

從幾何角度看，如果定義de為一個單位球，那么dT為一個橢球體，其半軸為辨識雅克比矩陣的奇異值[18]，如圖3所示。通過擴大橢球體的體積，可以提高標定位姿誤差對參數誤差的靈敏度。

圖3 奇異值的幾何表示圖Fig.3 Geometric interpretation of singular values

4.1 可觀測性指標

Menq和Borm[14-15]定義了第1個可觀測性指標O1，辨識雅克比矩陣奇異值的幾何平均值。通過最大化O1，可以增加橢球體的體積[18]。根據指標O1可以找到由機構參數誤差導致位姿誤差最大的位姿，從而使參數估計的效果更好。該指標的表達式如式(11)：

(11)

式中：m是測量位姿的數量，L為待識別的參數數量。

Driels和Pathre[16]提出了第2個指標O2，辨識雅克比矩陣條件數的倒數。最大化O2，可以提高辨識雅克比矩陣奇異值的一致性，提高了橢圓偏心率。該指標的表達式如式(12)：

O2=σL/σ1.

(12)

Nahvi和Hollerbach[17]提出了第3個指標O3，辨識雅克比矩陣奇異值的最小值。最大化O3，會增加橢球的最小半徑和體積。使所選擇的校準配置對參數誤差更敏感。該指標的表達式如式(13)：

O3=σL.

(13)

Nahvi和Hollerbach[18]提出了第4個指標O4，辨識雅可比矩陣的最小非零奇異值的平方除以最大奇異值。最大化O4，會增加橢球體的短軸，最大軸變小(即偏心率變小)。該指標的表達式如式(14)：

(14)

Sun和Hollerbach[19]提出第5個指O5，辨識雅可比矩陣的非零奇異值的調和平均值。該指標的表達式如式(15)：

(15)

4.2 辨識雅可比矩陣無量綱化

目前學者們采用辨識雅克比矩陣Jz的奇異值求解可觀測性指標，但是辨識雅可比矩陣是位移和姿態的函數，但位移和姿態對應的辨識雅可比矩陣中的元素的數量級不同，直接求解辨識雅可比矩陣得到可觀測性指標，獲得的測量配置會導致位移和姿態對測量誤差的魯棒性不同。因此，本文提出先對辨識雅可比矩陣進行無量綱化，再通過可觀測性指標選擇測量位姿，保證位移和姿態對測量噪音具有相同的魯棒性。

根據誤差模型公式(8)，可以將動平臺位置誤差矢量和姿態誤差矢量分開表示：

(16)

式中：Jp∈R3×42為位置誤差傳遞子矩陣，Jp為J的前3行；Jθ∈R3×42為姿態誤差傳遞子矩陣，Jθ為J的后3行。

當對m個位姿進行測量時，位置誤差傳遞矩陣為Jpm∈R3m×42， 姿態誤差傳遞矩陣為Jθm∈R3m×42。為了使Jpm與Jθm中元素數量級相等，對矩陣進行無量綱化：

(17)

Jzg即為無量綱化辨識雅克比矩陣。將無量綱化的辨識雅克比矩陣Jzg代替原有的辨識雅克比矩陣Jz，去求解可觀測指標Oi，再通過最大化指標Oi選取測量位姿。

5 測量位姿選擇算法

許多學者對基于可觀測指標Oi的位姿選擇算法進行了研究。其中，Daney等[20]提出的局部收斂和禁忌搜索的方法，具有不易陷入局部最優的特點。其工作原理及算法流程如圖4所示。

圖4 最優位姿選擇流程圖Fig.4 Flow chart of selecting the measurement poses

(1)生成待選位姿Ω0。由于任務工作空間為、互相耦合的六維，直接生成的位姿點不一定位于工作空間內，而關節空間是相互獨立的，因此通過關節空間6個桿長n-1等分，產生n6個桿長組合，再通過正解公式(2)獲得n6個位姿Ω0。

(2)設置初始位姿Ω1。在Ω0中任意選擇m個位姿作為初始位姿。

(5)終止條件。重復步驟(3)和步驟(4)，直到步驟(3)中添加的位姿T+為步驟(4)中被刪除的位姿T-。

6 數值仿真驗證

6.1 仿真流程

仿真流程包括選擇位姿、生成測量位姿的測量值、基于牛頓迭代的參數辨識過程、誤差補償、檢驗標定結果，如圖5所示，詳細具體過程如下。

(1)選擇測量位姿。設定Stewart并聯機構理論結構參數e(1)，根據圖4選擇測量位姿Tm，并通過運動學反解公式(3)得到測量位姿Tm的支腿長度L0。

(2)生成測量位姿的測量值。設定并聯機構誤差參數e0，與理論結構參數e(1)相加得到實際結構參數ea，根據實際結構參數ea和支腿長度L0正解出實際位姿Ta，在實際位姿Ta上添加測量噪聲Te獲得實際測量位姿Tam。

(4)誤差補償。直接對運動學模型進行補償，將辨識結構參數e(k+1)代替理論結構參數e(1)。

(5)標定結果檢驗。隨機給定一組位姿Ts反解出支腿長度，再根據實際結構參數e0+e(1)正解出實際位姿TJ，通過實際位姿TJ減去位姿Ts得到誤差參數辨識補償后的位姿誤差dTs。

圖5 仿真流程圖Fig.5 Flowchart of simulation

通過比較基于Jzg選取測量位姿的Stewart穩定平臺運動學標定位姿誤差dTs與基于Jz選取測量位姿的標定位姿誤差dTs的大小，來判斷哪種方法的標定精度更高。此外，還可以通過判斷最終的迭代位姿誤差dT(k)的大小、最終辨識誤差e(k)-e(1)與實際設定誤差e0差值的大小，來判斷哪種方法的標定精度更高。

6.2 數值驗證

給定一組六自由度并聯機構結構參數，如表1所示。對支腿進行二等分，求出36個候選位姿。

表1 六自由度并聯機構結構參數Tab.1 Theoretical structural parameters

設定標定位姿數m=18。分別基于未無量綱化的雅克比矩陣Jz和無量綱化的雅克比矩陣Jzg以可觀測性指標O1為指標，根據圖4選取測量位姿，其中可觀測性指標O1的迭代過程如圖6所示。

圖6 指標O1的迭代過程Fig.6 Values of O1 in the iterative

迭代18次后均可達到可觀測性指標O1的最大值。所選擇的測量位姿，如表2、表3所示。其中，對辨識雅克比矩陣進行無量綱化時，位移對應的辨識雅可比矩陣中的元素的數量比姿態大，特征長度K=600。

表2 基于Jz選取的測量位姿Tab.2 Measurement poses base on Jz

表3 基于Jzg選取的測量位姿Tab.3 Measurement poses base on Jzg

設置結構誤差參數e0，如表4所示，單位為毫米。正解求出18個實際位姿。設定3個水平的測量噪音，如表5所示。

表4 機構誤差參數e0Tab.4 Structure parameter error e0

表5 3個水平的測量噪音Tab.5 Measurement sensor noise of three levels

圖7 位姿誤差和結構誤差迭代過程Fig.7 Iterative process of pose and structure parameter error

在測量噪音1下，根據圖5迭代求解位姿誤差dT和結構參數誤差de，其迭代求解過程如圖7所示。迭代5次后迭代結果不再變化。表6給出了不同測量噪音下的辨識結果，其中P為位移，θ為姿態誤差，S為結構參數誤差。由表6可得，3種測量噪音下基于本文提出方法標定的結構參數精度相對于以前方法均有大幅提高，分別由1.007 5 mm提高到0.336 7 mm，0.100 9 mm提高到0.033 7 mm，0.010 1 mm提高到0.003 4 mm；三種測量噪音下基于本文提出方法標定的位置精度相對于以前方法基本不變，但姿態精度有大幅提高，分別由0.015 2°提高到0.003 3°，0.003 3°提高到0.000 3°，0.000 15°提高到0.000 033°，證明了本文方法的有效性。

表6 不同測量噪音下的辨識結果Tab.6 Iterative results under different measurement sensor noise

為了進一步驗證本文所提方法的有效性，表7、表8分別給出基于Jz和Jzg的測量噪音2下的結構參數誤差標定結果，并對運動學模型進行補償。隨機給定60組位姿，計算出位姿誤差，如圖8所示。可以看出位置精度基本不變，姿態精度大幅提高。

表7 基于Jz的測量噪音2下結構參數誤差標定結果Tab.7 Identified structural parameter error base on Jz

表8 基于Jzg的測量噪音2下結構參數誤差標定結果Tab.8 Identified structural parameter error base on Jzg

圖8 60組隨機位姿下的位姿誤差

表9～表12給出了基于可觀測性指標O2～O5選取測量位姿的標定結果。由表9～表12可得，不同可觀測性指標下，基于本文提出方法標定的位置精度相對于以前方法基本不變或有所下降，但姿態精度和結構參數精度均有大幅提高，證明了本文方法的有效性。

表9 基于可觀測性指標O2的辨識結果Tab.9 Iterative results under observability index O2

表10 基于可觀測性指標O3的辨識結果Tab.10 Iterative results under observability index O3

表11 基于可觀測性指標O4的辨識結果Tab.11 Iterative results under observability index O4

表12 基于可觀測性指標O5的辨識結果Tab.12 Iterative results under observability index O5

7 標定實驗

標定實驗與標定仿真的流程相同，主要包括選擇測量位姿、牛頓迭代求解辨識參數、誤差補償和檢驗標定結果。其中，對于測量位姿的選擇，Stewart并聯機構的理論結構參數與仿真設定的結構參數相同，因此選用表3基于Jzg選取的測量位姿作為標定實驗的測量位姿。

對于測量位姿，本實驗采用徠卡AT960激光跟蹤儀(定位精度為5 μm+6 μm/m)對Stewart穩定平臺進行位姿測量，首先測量出上平臺坐標系{p}和下平臺坐標系{e}位于激光跟蹤儀坐標系{g}中的位姿，然后再通過坐標變換獲得上平臺坐標系{p}相對于下平臺坐標系{e}的位姿，如圖9所示。

圖9 基于激光跟蹤儀的標定實驗Fig.9 Calibration experiment using a laser tracker

激光跟蹤儀只能測量空間某一點位于激光跟蹤儀坐標系中的位置，需要上、下平臺上多個測量點才能計算出上平臺坐標系{p}和下平臺坐標系{e}位于激光跟蹤儀坐標系{g}中的位姿。穩定平臺的上平臺坐標系{p}、下平臺坐標系{e}和測量點分布，如圖10所示。

圖10 上下平臺的測量點分布Fig.10 Measurement point distribution of the platform

(18)

(19)

式中：Atan 2(x，y)是雙變量反正切函數，γ為繞x軸的轉角，β為繞y軸的轉角，α為繞z軸的轉角。

根據測量得到的位姿與理論位姿的差值進行牛頓迭代求解，辨識出機構參數的誤差值，進行運動學模型補償。隨機給出20組位姿，進行標定結果的檢驗，圖11給出了標定前后的位姿誤差。位置和姿態誤差的均值分別從2.321 mm降至0.242 mm，0.246°降至0.025°。標定后的位姿精度有了明顯提高，但距離仿真標定精度還有差距，主要是本文沒有考慮關節間隙、機構剛度、位姿控制精度等對位姿誤差的影響。

圖11 位姿誤差標定前后對比Fig.11 Pose error contrast before and after calibration

8 結 論

本文提出對辨識雅克比矩陣進行無量綱化，再根據基于無量綱化辨識雅克比矩陣的可觀測性指標選擇測量位姿，保證位移和姿態對測量噪音具有相同的魯棒性。通過數值算例驗證了，3種測量噪音下該方法標定的結構參數精度相對于傳統方法均有大幅提高；該方法標定的位置精度相對于傳統方法基本不變，但姿態精度有大幅提高，證明了該方法的有效性。

在以該方法選取的測量位姿對Stewart并聯機構的標定試驗中發現，位姿精度有了明顯提高，但距離仿真標定精度還有差距，主要是本文沒有考慮關節間隙、機構剛度、位姿控制精度等對位姿誤差的影響，后續將對其進行研究，以提高標定試驗精度。