基于Brent算法的軸偏置虎克鉸型Hexapod平臺位置反解

孫佳霖，劉曰濤，楊夢超，韓 振，付連壯

（山東理工大學 機械工程學院，山東 淄博255049）

1 引言

Hexapod并聯平臺也稱作并聯六自由度平臺，其基本構型最早由英國工程師Stewart提出。經過幾十年的發展，Hexapod平臺憑借控制精度高、結構剛性好、負載能力強、工作穩定性高等特點，被廣泛應用于光學領域［1-2］、主動隔振［3-4］、運動模擬［5］及自動對接等領域。目前，Hexapod平臺也是大型光電望遠鏡次鏡調整機構以及空間相機隔震系統的主要構型形式。位于智利的VISTA（Visible and Infrared Survey Telescope for Astronomy）望遠鏡［6］，LSST（The Large Syn?optic Survey Telescope）望 遠 鏡［7］，中 國“天 眼”FAST望遠鏡［8］以及詹姆斯?韋伯空間望遠鏡（JSMT）等均采用了Hexapod并聯平臺作為自身重要調整機構。

Hexapod并聯平臺主要構型形式包括6-SPS，6-UPS，6-UCU（S，P，U，C分別代表球鉸、移動副、虎克鉸、圓柱副）等［9-11］。由于望遠鏡調整機構的整體趨勢是向著高剛度高穩定性方向發展，所以球鉸的負載能力和運動間隙成為了其主要制約因素［12］。同球鉸相比，虎克鉸在損失了一定工作空間的前提下，提高了結構整體穩定性和控制精度。同時，為了進一步提高虎克鉸的剛性，Gloess［13］等 提 出 了 軸 偏 置 虎 克 鉸 結 構，使 得Hexapod并聯平臺具有更好的結構剛性和穩定性。

Hexapod并聯平臺的位置反解是根據動平臺的位置姿態求解驅動腿長度的過程，不同結構形式的Hexapod平臺，其位置反解算法的難易程度也不同。球鉸和十字交叉軸虎克鉸結構均具有唯一確定的旋轉中心，且旋轉中心在相對坐標系下不發生改變。因此，6-SPS型，6-UPS型或6-UCU型Hexapod平臺的位置反解可以通過解析法直接求解［14-15］。由于軸偏置虎克鉸結構具有兩個垂直且不相交的旋轉軸，因此，運動鏈的長度不僅與直線驅動部件有關，而且與軸偏置虎克鉸結構的位置姿態有關。同時，軸偏置虎克鉸作為從動件，無法確定兩旋轉軸的轉動角度，所以該類型平臺的位置反解無法通過解析法直接求得，只 能 采 用 數 值 法。Dalvand［16］等 針 對6-RRCRR（R表示轉動副）型并聯平臺，利用閉環矢量法建立了運動鏈的非線性方程組，并通過Newton-Raphson法對方程 組進行求 解。Yu［17］，Han［18］等對該算法控制下的并聯平臺進行了運動學分析和測試。該方法雖然可以解決軸偏置虎克鉸型Hexapod平臺的位置反解問題，但是在工程實際應用中，大量的位置反解計算對平臺響應速度影響較大。因此，對于雙端軸偏置虎克鉸型Hexa?pod平臺的位置反解還需要尋求既快又準的計算方法。

本文針對雙端軸偏置虎克鉸型Hexapod平臺，提出了一種新的位置反解算法。首先，利用空間向量法構建了基于空間圓輔助模型的運動學模型；然后，結合Brent法求解平臺位置反解的數值解，并對絲杠螺母產生的衍生運動誤差進行了補償；最后，建立位置正解模型并搭建實驗樣機，以驗證所提出算法的準確性和可靠性。

2 Hexapod平臺理論模型

2.1 軸偏置虎克鉸結構

虎克鉸結構具有承載能力強、結構剛度高、運動間隙小等特性，被廣泛應用于各種并聯機構的轉動關節。但是典型的十字軸虎克鉸結構，構型較為復雜，十字交叉軸的設計對于加工和裝配都是巨大的考驗。因此，提出了虎克鉸結構的優化形式——軸偏置虎克鉸結構，如圖1所示。兩轉軸R1，R2分別與平臺和驅動腿連接，并通過軸承與轉動塊鉸接。相比較于十字軸虎克鉸，該結構具有更高的剛度、強度和更大的工作空間，且兩轉軸和轉動塊相互分離的設計方案，更有利于保證加工精度和裝配誤差。

圖1 軸偏置虎克鉸結構Fig.1 Axis offset Hooke joint

2.2 平臺模型

雙端軸偏置虎克鉸型Hexapod平臺由動平臺、定平臺、6根驅動腿和12個軸偏置虎克鉸組成，驅動腿由滾珠絲杠和驅動電機等部件組成。據此，Hexapod平臺的理論模型可視為動定平臺通過6根RRCRR運動鏈連接的并聯機構，如圖2所示。

軸偏置虎克鉸位于動、定平臺的鉸點Ri1和Ri4（i=1，2，…，6）分別構成了兩個不同的基準圓，稱為動基準圓和定基準圓。位于動基準圓和定基準圓上的鉸點均圍繞120°基準線對稱分布。為描述平臺的運動位姿，分別定義基坐標系OB，工作坐標系OW和工具坐標系OT。基坐標系OB位于定準圓圓心，與定平臺固接，ZB軸始終垂直于定基準圓所在平面且指向動平臺方向。工作坐標系OW始終位于基坐標系ZB軸方向H處，指向與基坐標系相同，不隨平臺運動而變化。平臺位于零點狀態時，工作坐標系原點與動基準圓圓心重合。工具坐標系OT固接于動平臺，隨動平臺的運動而運動，坐標原點與動基準圓圓心重合。因此，鉸點Ri4（i=1，2，…，6）在基坐標系下的位置向量BR i4（i=1，2，…，6）可表示為：

其中：φ表示定平臺上相鄰兩鉸點間的夾角，RB為定基準圓半徑，如圖2所示。

圖2 Hexapod平臺理論模型Fig.2 Theoretical model of hexapod platform

鉸點Ri1（i=1，2，…，6）在工具坐標系下的位置向量TR i1（i=1，2，…，6）可表示為：

其中θ表示動平臺上相鄰兩鉸點間的夾角，如圖2所示。

3 Hexapod平臺位置反解算法

3.1 位置反解基本算法

機構的位置分析是機構速度分析、加速度分析、受力分析、動力分析和機構綜合等分析的基礎，它包括位置正解和位置反解兩部分。位置正解是根據輸入端的輸入參數求解輸出端的位置和姿態的過程；反之根據輸出端的位置和姿態求解輸入端參數的過程則是位置反解。因此，由平臺理論模型可知，驅動腿的長度可表示為（如圖2所示）：

其中：BR i2和BR i3（i=1，2，…，6）分別表示鉸點Ri2和Ri3（i=1，2，…，6）在 基 坐 標 系OB下 的 位 置向量。

根據所建立的坐標系，動平臺的位置和姿態可表示為矢量［x，y，z，u，v，w］T。其中，［x，y，z］T表示工具坐標系OT在工作坐標系OW下的位置，［u，v，w］T表示動平臺分別繞工具坐標系XT，Y T，ZT軸的旋轉角度。工具坐標系OT相對于工作坐標系OW的姿態變換則用RPY組合變換來表示。因此，可得工具坐標系OT相對于工作坐標系OW的變換矩陣：

通過坐標變換方法，可得鉸點Ri1（i=1，2，…，6）在基坐標系下的位置向量：

由Hexapod平臺的理論模型可知，軸偏置虎克鉸的兩個轉動量均屬于未知量，所以無法通過解析法求得驅動腿的長度。分析軸偏置虎克鉸的運動特性可知，一個鉸點始終圍繞另一個鉸點在同一平面內做圓周運動。以鉸點Ri1（i=1，2，…，6）為圓心，偏置距離e為半徑，作輔助圓C1，其法向量N i1（i=1，2，…，6）與上轉軸軸心重合。以鉸點Ri4（i=1，2，…，6）為圓心，偏置距離e為半徑，在下轉軸的垂面內作輔助圓C2，其法向量為N i4（i=1，2，…，6），如圖3所示。因此，驅動腿長度li可等價為空間中兩個圓的最短距離，進而將位置反解求解問題等效為空間中兩圓最短距離的求解問題。David［19］提出了求解空間中圓與直線、圓與圓的距離求解方法。

圖3 Hexapod平臺輔助圓模型示意圖Fig.3 Schematic diagram of auxiliary circle model of hexapod

為建立空間中任意兩圓的距離公式，首先，假設空間中任意點向量P，點P至圓C2的距離可表示為：

其中N i4為空間圓C4的法向量。

空間圓C1可表示為：

式中i=1，2，…，6，U i1和V i1為空間圓C1的任意兩個單位向量。

令聯立公式（6），（7）和（8），得空間圓C1和空間圓C2的距離公式：

式中：

其中i=1，2，…，6。驅動腿長度可表示為：

因此，可利用公式（9）的導函數對最小值進行求解。

3.2 衍生運動補償

本文所涉及雙端軸偏置虎克鉸型Hexapod平臺，驅動單元包括滾珠絲杠和驅動電機等部件。其中，滾珠絲杠具有兩個相互耦合的自由度，包括螺母的直線運動和旋轉運動。螺母的直線運動作為主動運動，由電機直接控制，為可控運動。螺母的旋轉運動作為驅動腿的第6個自由度，需要配合動平臺完成運動。然而，螺母的旋轉運動會導致驅動腿長度發生變化，且無法通過驅動裝置直接控制。因此，該運動被定義為衍生運動，需要在主動運動的基礎上進行衍生運動誤差補償。從而驅動腿的實際長度可表示為：

其中：li定義為伸縮腿理論長度，Δli是由螺母的旋轉運動所產生的衍生運動誤差。

定義鉸點Ri1，Ri2，Ri3，Ri4的旋轉軸向量分別為S i1，S i2，S i3，S i4，根據圖2所示的驅動腿結構示意圖可知：

其中i=1，2，…，6。

將位置反解求得的ψi值代入公式（5）可得鉸點Ri2的位置向量，根據空間中點到圓的最短距離可求得Ri3的位置向量。又因S i1=N i1，S i4=N i4。向量S i2和S i3可表示為：

到達目標位置時，螺母轉動的角度可表示為：

因此，由螺母的旋轉運動所產生的衍生運動誤差為：

其中：δ0為平臺位于初始狀態時S i2和S i3的夾角，D為絲杠的導程。

4 Brent算法

Brent方法是一種結合二分法、割線法和反二次插值方法的尋根方法［20］。二分法則是不斷對區間［a，b］進行二等分，利用端點對零點進行逼近，即下次迭代的初值可表示為：

割線法是根據有根區間內a，b兩點所構造弦的斜率代替目標函數的切線斜率，將割線與橫坐標的交點作為方程的近似根。即給定區間內兩點（a，f（a）），（b，f（b）），建立割線方程：

令f(x)=0，則下次迭代的初值為：

反二次插值類似于割線法，是根據區間中三個點（a，f（a）），（b，f（b）），（c，f（c））確定一個二次函數，并將二次函數與縱坐標的交點作為下次迭代的初值：

Brent法的核心是選擇盡可能快的方式求根，如果生成的結果超出了估計值范圍，則轉為相對保守的二分法，即使二分法的收斂速度比較慢，但可以有效的將生成的根估計值鎖定在邊界以內，如果生成了可以接受的根則轉為更快的反二次插值法或割線法。

Brent算法的計算流程，首先初始化區間（a，b），使得f(a)f(b)＜0，并滿足|f(a)|＞|f(b)|。每次迭代的初值包括ak，bk，bk-1，bk-2（ak為第k次迭代的左估計值，bk為右估計值，其中k=0，1，2，…）。|ε|＞|bk-bk-1|且上次迭代為二分法（ε為迭代精度）；|s-bk|＞且上次迭代為二分 法；|ε|＞|bk-1-bk-2|且 上 次 迭 代 不 是 二 分法；|s-bk|＞-bk-2|且上次迭代不是二分法；，bk)且上次迭代不是二分法，若滿足以上條件的任意一個則本次迭代采用二分法。若采集點滿足f(ak)≠f(ck)≠f(bk)，則采用反二次插值，否則采用割線法。

5 Brent算法在Hexapod并聯平臺位置反解中的應用

Brent算法在Hexapod并聯平臺位置反解中主要應用于求解鉸點Ri2，Ri3的位置向量，流程圖如圖4所示，具體計算流程如下：

圖4 Hexapod平臺位置反解流程圖Fig.4 Flowchart of inverse kinematics of Hexapod plat?form

（1）給定平臺初始參數，包括H，φ，θ，RM，RB等。預設當前計算精度ε，迭代初值ψ0，最大迭代次數N；

（2）輸入目標位姿（x，y，z，u，v，w）；

（3）求解變換矩陣T；

（4）根據坐標變換方法，求解各鉸鏈變化后的位置向量、法向量、單位向量TR i1，N i1，U i1，V i1；

（5）建立空間圓距離公式f（ψi）；

（6）求解距離導數公式f′（ψi）；

（7）判斷區間（a，b）是否有根且迭代次數小于N，如果是轉至步驟（8），否則轉至步驟（19）；

（8）根據f（b）及f（a）值的大小判斷是否交換端值，并令c=a，設置標志位M=0；

（9）判斷有根區間大小是否小于迭代精度ε，或f′（b）=0，如果是轉至步驟（15），否則轉至步驟（10），進入Brent算法；

（10）判斷f（a），f（b），f（c）是否相等，是轉至（11），否轉至（12）；

（11）利用反二次插值求解，得下次迭代值s；

（12）利用割線法求解，得下次迭代值s；

（13）判斷是否符合二分法求解條件（第4章介紹），若符合，采用二分法求得下次迭代值s并使標志位置1，否則標志位置0；

（14）根據f（a）*f（s）的值重新分配有根區間。迭代次數增加并返回步驟（9）；

（15）將求得的ψi值帶回到空間圓距離公式f（ψi），求得驅動腿理論長度li；

（16）求解鉸點Ri2，Ri3的位置向量，并根據向量叉乘原理求解法向量S i2，S i3；

（17）求解衍生運動誤差Δli；

（18）得到驅動腿實際長度L i；

（19）迭代終止，無解或超出最大迭代次數。

6 實驗與測試

搭建Hexapod并聯平臺實驗樣機，其結構參數為，H=330 mm，φ=9°，θ=19°，RM=150 mm，RB=315 mm，e=13 mm，D=1。驅動腿運動范圍：285～360 mm。運動空間：x=±20 mm，y=±20 mm，z=±25 mm，u=±5°，v=±5°，w=±8°。該構型在其工作空間內無奇異點。

在Windows7操作系統，Intel（R）Xeon（R）CPU E3-1225 v5@3.30 GHz 3.31 GHz處理器，8.00 GB內存的電腦上，利用Matlab（R2016b）對Newton-Raphson法和Brent法位置反解算法進行編程。迭代精度均設置為δ=0.00001，最大迭代次數N=10000。

隨機選取工作空間中的任意5組位姿（如表1所示），并利用Newton-Raphson法和Brent法分別進行位置反解求解，迭代次數和迭代時間如表2所示。分析表2可知，雖然Newton-Raphson算法的迭代次數較少，但是其消耗的計算時長遠大于Brent法。這是由于基于Newton-Raphson算法的位置反解求解方法，需要利用閉環矢量法對同一運動鏈建立兩個不同的約束方程，且需求解至少12個轉換矩陣和6個Jacobian矩陣［16-17］。因此，在計算的復雜程度上，基于Newton-Raphson算法的求解方法要高于采用Brent算法的求解方法。

表1 隨機選取5個位姿Tab.1 Five randomly selected poses

表2 迭代次數和迭代時間對比Tab.2 Comparison of iteration number and iteration time

為驗證本文所提出算法的準確性，參照已有的位置正解算法［21］，對該平臺進行位置正解建模。將位置正解算法作為反饋機制，用來實時檢測平臺的運動軌跡。設定平臺運動路徑為：

計算位置反解，并將計算結算結果作為位置正解的輸入量，求解平臺的實時位姿，結果如圖5所示。

圖5 Hexapod平臺位姿變化Fig.5 Pose changes of the Hexapod platform

根據以上實驗路徑，對衍生運動誤差補償前后的驅動腿長度誤差和平臺位姿誤差進行分析。由圖6可見，驅動腿長度誤差可達到0.015 mm。分析圖7，由Hexapod平臺的位置正解模型反饋回的動平臺位姿狀態可知，衍生運動產生的腿長誤差，不僅會引起目標自由度的位置或角度偏差，而且其他自由度也會產生較大偏差，且誤差超過0.01 mm/0.36"。因此，對于高精度Hexapod平臺，衍生運動誤差補償環節是必不可少的。

圖6 Hexapod平臺驅動腿長度誤差Fig.6 Leg length error of hexapod platform

圖7 Hexapod平臺位姿誤差Fig.7 Pose error of hexapod platform

按照以上實驗路徑，對實驗樣機的響應速度進行測試，在不改變平臺硬件設施，以及軌跡規劃、路徑規劃、運動學正解等計算程序的前提下，僅對比兩種不同求解方法（基于Brent法和基于Newton-Raphson法）控制下實驗樣機的響應速度。到達每一目標位姿時程序執行位置反解子程序的次數，以及從發送指令到平臺移動之前所消耗的時長均記錄于表3。分析表3可知，在相同計算次數下，基于Brent算法法控制下的Hexapod平臺響應速度比基于Newton-Raphson算法控制下的平臺響應速度高出約0.5 s（含硬件設備的性能、通訊的延遲時間等因素）。因此，基于Brent算法的并聯六自由度Hexapod平臺位置反解有效改善了該類型平臺的響應速度。

表3 平臺響應速度對比Tab.3 Comparison of platform response speed

同時，采用Renishaw XL-80激光干涉儀對實驗樣機進行打表測試，以驗證平臺仍保持有微米級和角秒級控制精度。選取重復精度較差的x軸和u軸進行重復精度測量，并按照ISO 230-2標準進行重復定位精度計算。測量結果選取4倍標準差作為該測量軸的重復定位精度。測量結果如圖8～圖9所示。測試結果顯示移動軸單向重復定位精度可達2.0μm且轉動軸單向重復定位精度可達1.5。

圖8 x軸重復精度測量結果Fig.8 x-axis repeatability measurement results

圖9 u軸重復精度測量結果Fig.9 u-axis repeatability measurement results

7 結論

本文針對雙端軸偏置虎克鉸型并聯六自由度Hexapod平臺提出了一種基于Brent算法的位置反解求解方法。該方法建立了一種基于空間圓輔助模型的運動學模型，并利用Brent法對非線性方程組進行數值求解，從而在保證計算精度的同時提高了運算速度。通過實驗與測試對所提出算法的有效性和準確性進行了評估，實驗結果表明，基于Brent算法的位置反解算法運算時間優于Newton-Raphson算法，且該控制算法下的實驗樣機在保證精度的同時平臺綜合響應速度提高了約0.5 s。