基于6-PUS 并聯(lián)機構(gòu)的直升機升力部件裝配控制系統(tǒng)設(shè)計

孫 嚴(yán)，卜 泳，杜兆才，陳 龍，段巍

（中國航空制造技術(shù)研究院，數(shù)字 化制造技術(shù)航空科技重點實驗室，北京 100024）

0 引言

直升機升力系統(tǒng)關(guān)鍵部件自動化裝配系統(tǒng)主要由龍門結(jié)構(gòu)、并聯(lián)機構(gòu)、模塊化夾持工裝、被動柔順定位平臺組成。其中并聯(lián)機構(gòu)（圖1）具有六自由度的調(diào)姿定位功能，配合模塊化夾持工裝對不同型號直升機自動傾斜器和主槳轂的夾持，實現(xiàn)以主減速器為目標(biāo)的精確調(diào)姿和自動化插合裝配。

圖1 升力系統(tǒng)裝配并聯(lián)機構(gòu)

采用并聯(lián)機構(gòu)來完成調(diào)姿裝配，主要是由于其具有結(jié)構(gòu)穩(wěn)定、剛性高、低慣量、高精度等特點[1]，可實現(xiàn)3 個自由度的平移和3 個自由度的旋轉(zhuǎn)，能夠靈活地調(diào)整產(chǎn)品的位置和姿態(tài)來完成插合裝配。由于并聯(lián)機構(gòu)的驅(qū)動桿長與動平臺位姿之間存在非線性的關(guān)系，這就使得其控制非常復(fù)雜[2]。為了實現(xiàn)并聯(lián)機構(gòu)動平臺的調(diào)姿控制，就要解決其運動學(xué)的正、反解問題。雖然目前并聯(lián)機構(gòu)的正、反解算法很多，但考慮到運動的連續(xù)性，有些解是實際運動過程中不可能出現(xiàn)的[3]。同時，為了滿足控制過程的實時要求，對算法的運算效率和可達性也提出了要求。如何高效、快速、實時求得并聯(lián)機構(gòu)可以到達的可行解，并在運動控制器中運行是一個難題。

對于多數(shù)常用的運動控制器來說，其主要關(guān)注點在于協(xié)調(diào)多個電機完成指定的運動，比較著重軌跡規(guī)劃、速度規(guī)劃等，比如協(xié)調(diào)多個軸完成插補動作，而對算法功能支持較少。往往算法的實現(xiàn)更多的是依賴MATLAB或者高級語言，利用其強大的數(shù)學(xué)運算能力來建立數(shù)學(xué)模型。這樣就產(chǎn)生了一個問題，即生成的模型難以和運動控制器有效結(jié)合，因此控制執(zhí)行機構(gòu)的實時性也就很難保證。TwinCAT3 是倍福公司運動控制系統(tǒng)的配套軟件，是一款基于Windows 平臺的實時控制系統(tǒng)，支持符合IEC61131-3 的PLC 語言以及C++ 的混合編程[1]，可以實現(xiàn)對伺服系統(tǒng)的實時控制。基于Twin-CAT3 軟件的這一特點，運動軌跡的規(guī)劃和伺服軸的控制可以借由PLC 來實現(xiàn)，而正、反解的數(shù)學(xué)模型則可以通過C++來實現(xiàn)，并通過TwinCAT3 軟件的TcCom Object 功能生成可實時運算的模塊，這樣就使得計算時間非常短，從而滿足實時控制的需要。

結(jié)合上述思路，本研究提出了一種6-PUS 并聯(lián)機構(gòu)的運動控制方法。首先求解出并聯(lián)機構(gòu)的位置反解，然后通過牛頓迭代法，利用位置反解來求解位置正解，該方法可以保證求得的解是并聯(lián)機構(gòu)的可達解[3]。在Twin CAT3 軟件中建立正、反解的數(shù)學(xué)模型來進行實時求解，最終根據(jù)實時求得的桿長，運動控制器通過高速Ether-CAT 總線驅(qū)動6 個伺服電機實現(xiàn)精確協(xié)調(diào)同步運動控制。

1 運動學(xué)分析

1.1 總體結(jié)構(gòu)

并聯(lián)機構(gòu)由定平臺、移動副、連桿、電機、球鉸、動平臺等組成，動平臺與力傳感器、快換機構(gòu)、夾持工裝連接，定平臺與龍門結(jié)構(gòu)上橫梁連接。考慮到裝配工作需要，并聯(lián)機構(gòu)應(yīng)具有較大的向行程，因此，選擇并聯(lián)機構(gòu)的構(gòu)型為6-PUS，如圖2 所示，即定平臺與動平臺之間通過6 根連桿連接，對于每根連桿構(gòu)成的運動支鏈，P表示移動副，U表示虎克鉸，S表示球副。

圖2 6-PUS 并聯(lián)機構(gòu)構(gòu)型

6-PUS 并聯(lián)機構(gòu)的機座B1B2B3B4B5B6構(gòu)成并聯(lián)機構(gòu)的定平臺，6 條固定導(dǎo)軌垂直于定平臺，滑塊沿導(dǎo)軌滑動。6 個運動支鏈的每個支鏈為一個定長的連桿，一端通過虎克鉸與滑塊連接，另一端通過球鉸與動平臺A1A2A3A4A5A6的6 個頂點連接。滑塊與定平臺之間構(gòu)成移動副，當(dāng)滑塊在固定導(dǎo)軌上的位置發(fā)生變化時，虎克鉸、連桿、球副傳遞到動平臺，耦合作用產(chǎn)生動平臺位置和姿態(tài)的改變。

滑塊的運動通過6 個伺服電機來實現(xiàn)，伺服驅(qū)動器與安裝在固定導(dǎo)軌上的絕對值光柵尺構(gòu)成全閉環(huán)控制。運動控制器采用倍福C6920 系列基于Windows 平臺的PC 控制器，CPU 為Intel 酷睿i5 四核處理器，運行TwinCAT3 軟件，經(jīng)EtherCAT 總線與節(jié)點上的6 個伺服驅(qū)動器進行通訊，借由控制伺服電機的實時精確同步運動，實現(xiàn)動平臺在三維空間6 個自由度（X，Y，Z，α，β，γ）的運動。

1.2 位姿反解

為了便于描述，如圖2 所示，在定平臺B1B2B3B4B5B6上定義一個全局坐標(biāo)系Ob-XbYbZb作為基礎(chǔ)坐標(biāo)系，在動平臺A1A2A3A4A5A6上定義一個局部坐標(biāo)系Op-Xp Yp Zp作為活動坐標(biāo)系。定平臺原點Ob和動平臺原點Op分別位于其幾何中心，各坐標(biāo)軸與全局坐標(biāo)系的同名坐標(biāo)軸方向相同。

6-PUS 并聯(lián)機構(gòu)運動學(xué)反解的求解過程就是已知動平臺的位姿，解算出滑塊的運動量，具有唯一解。對于實際運動控制來說，控制系統(tǒng)需要通過路徑規(guī)劃算法給出每個PLC 任務(wù)周期內(nèi)動平臺在空間中的位姿值（X，Y，Z，α，β，γ），然后根據(jù)反解算法，實時計算出每個滑塊所需要移動的距離，并由伺服電機驅(qū)動各滑塊執(zhí)行同步運動。

以其中的一個運動支鏈為例，可以繪制出圖3 的矢量關(guān)系，其中矢量Ob Bi表示定平臺與導(dǎo)軌連接點Bi在全局坐標(biāo)系Ob-Xb Yb Zb中的位置，Ob Bi= [xBi yBi0]T；矢量Bi Pi表示滑板中心Pi相對于點Bi的位置，Bi Pi=[0 0zPi0+zPi]T，zPi0為滑板中心Pi的初始位置（零點）到點Bi的距離，zPi為滑板相對于零點的運動量；矢量Pi Ti表示滑板中心Pi相對于虎克鉸中心Ti的位置，虎克鉸中心與滑板中心等高，因此Pi Ti= [xTiPi yTiPi0]T；矢量OP Ai表示球鉸與動平臺的連接點Ai在局部坐標(biāo)系Op-Xp Yp Zp中的位置，OP Ai=[xAi yAi0]T；Ob OP表示全局坐標(biāo)系Ob-Xb Yb Zb相對局部坐標(biāo)系Op-Xp Yp Zp的位置矢量，Ob OP=[xbP0+ΔxP ybP0+ΔyP zbP0+ΔzP]T，（xbP0ybP0zbP0）為動平臺的初始坐標(biāo)位置，（ΔxPΔyPΔzP）為動平臺的位移；Ai Si表示球鉸中心Si相對于點Ai的位置，Ai Si=[0 0zSi]T，zSi為球鉸中心相對于動平臺端面的偏移量；Ti Si表示虎克鉸中心相對球鉸中心的位置，其模長為連桿長度Li，為一固定值。

圖3 6-PUS 并聯(lián)機構(gòu)結(jié)構(gòu)及支鏈的矢量關(guān)系

由于OPAi+AiSi為局部坐標(biāo)系Op-Xp Yp Zp下的矢量，因此需要通過旋轉(zhuǎn)變換矩陣R 轉(zhuǎn)換為全局坐標(biāo)系Ob-Xb Yb Zb下的矢量。如果按照先繞X軸旋轉(zhuǎn)歐拉角α，再繞Y軸旋轉(zhuǎn)β，最后繞Z軸旋轉(zhuǎn)γ的順序，可以得到旋轉(zhuǎn)矩陣。

將各變量代入式（3）可得：

其中，ΔxP、ΔyP、ΔzP、α、β、γ為動平臺位姿的輸入量，其他變量為已知值或可求值，因此由式（5）可以計算出反解值，即滑塊運動量zPi。

1.3 位姿正解

已知各滑塊的位置求解動平臺位姿的方法稱為并聯(lián)機構(gòu)的位置正解。由于6-PUS 并聯(lián)機構(gòu)結(jié)構(gòu)的復(fù)雜性，在求解位置正解時需要對多元非線性方程組進行求解，因此比較復(fù)雜，并且存在多個解。目前常用的正解解法為解析法和數(shù)值法。解析法是通過約束方程消去未知數(shù)，使方程成為只含有一個未知數(shù)的高次多項式[4]。其優(yōu)點在于能夠得到全部正解，其缺點在于運算量大、計算時間長，且有些解是機構(gòu)連續(xù)運動過程中不可能出現(xiàn)的。而數(shù)值法則避免了解析法的上述問題，其不但計算速度較快、求解效率高[5]，更重要的是可以保證所求的解是唯一的、最優(yōu)的，是機構(gòu)可以到達的可行解，這樣就滿足了運動控制過程中對于實時性和連續(xù)性的要求。

采用數(shù)值解法中的牛頓迭代法，通過反解計算得到的滑塊位置，經(jīng)反復(fù)迭代逼近目標(biāo)滑塊位置，最終得到位置正解。由于并聯(lián)機構(gòu)滑塊所在的固定導(dǎo)軌安裝有絕對值光柵尺，因此通電后光柵尺讀數(shù)即為滑塊所在的位置，經(jīng)過位置正解就可以求出動平臺在空間中的位姿。正解算法流程如圖4 所示。這時經(jīng)過n+1 次迭代后得到的位姿值Pn+1即為目標(biāo)滑塊位置為zP時對應(yīng)的位姿值。

圖4 正解算法流程圖

2 控制系統(tǒng)軟硬件設(shè)計

2.1 硬件組成

由于6-PUS 并聯(lián)機構(gòu)在運動時需要驅(qū)動六個滑塊完成精確同步運動，因此在硬件上選用了倍福C6920 系列基于Windows 平臺的PC 控制器，它可以通過EtherCAT 總線與節(jié)點上的六個AX5201 伺服驅(qū)動器進行通訊。EtherCAT 總線基于工業(yè)以太網(wǎng)，具有強大的分布式時鐘功能，在多個伺服軸需要同時執(zhí)行協(xié)調(diào)運動時可以確保相互間的同步時間偏差小于1μs，從而實現(xiàn)精確同步控制。

控制系統(tǒng)結(jié)構(gòu)如圖5 所示，運動控制器與伺服驅(qū)動器和I/O 模塊之間通過工業(yè)以太網(wǎng)進行連接；集成控制軟件與控制器之間通過TwinCAT3 的ADS 接口進行通訊；I/O 模塊用于獲取急停和限位狀態(tài)，并配合控制器完成相應(yīng)的I/O 邏輯控制，對報警狀態(tài)進行輸出。

圖5 控制系統(tǒng)結(jié)構(gòu)

2.2 軟件設(shè)計

2.2.1 TwinCAT3 PLC 程序調(diào)用C++算法模塊

TwinCAT3 軟件支持PLC 和C++的混合編程，利用C++可以更容易、更方便地實現(xiàn)上文所述的正反解算法，并可以將其封裝成TcCOM 組件供PLC 程序進行調(diào)用，而PLC 程序則可以專注于實現(xiàn)運動控制功能。

圖6 在TwinCAT3 中注冊C++接口并發(fā)布TcCOM 組件

PLC 調(diào)用C++正反解算法模塊的過程是：首先創(chuàng)建C++正反解算法項目，添加TwinCAT Module Class with Cyclic IO，并在TMC Files 下創(chuàng)建輸入/輸出變量，生成TMC 代碼。創(chuàng)建C++正反解所需的系統(tǒng)Task，并將Task分配給編譯好的C++接口TcCOM 組件。然后將C++程序與PLC 程序進行鏈接，在PLC 程序中新建滑塊位置值和位姿值變量并鏈接到C++程序中對應(yīng)的變量上去。這樣就實現(xiàn)了C++ 算法模塊實時進行正、反解計算，PLC 程序根據(jù)解算值控制各執(zhí)行機構(gòu)同步運動的目的。

2.2.2 基于TwinCAT3 的控制軟件設(shè)計

6-PUS 并聯(lián)機構(gòu)在運動時需要控制系統(tǒng)驅(qū)動6 個滑塊完成精確同步運動，即根據(jù)給定動平臺的目標(biāo)位姿X、Y、Z、α、β、γ，實時計算出每個滑塊的運動量并同步運動。為了達到這一目的需要用到TwinCAT3 NCI 來實現(xiàn)多軸插補聯(lián)動。在TwinCAT3 中添加NC 插補通道和虛擬軸X、Y、Z、A、B、C，6 個虛擬軸分別對應(yīng)動平臺的位姿X、Y、Z、α、β、γ，再添加6 個滑塊坐標(biāo)系物理軸L1、L2、L3、L4、L5、L6 鏈接到伺服驅(qū)動器和PLC。從上文已知可以在TwinCAT3 軟件中調(diào)用C++正反解算法模塊，因此將六個滑塊軸L1、L2、L3、L4、L5、L6 對應(yīng)的光柵尺讀數(shù)作為輸入變量鏈接到正解模塊，可以計算出動平臺在笛卡爾坐標(biāo)系中的位姿值，并輸出給NCI 的6 個虛擬軸X、Y、Z、A、B、C。將目標(biāo)位姿X、Y、Z、α、β、γ作為輸入量給到虛擬軸，則可以通過反解模塊，計算出滑塊所需要的移動量，作為輸出變量通過外部給定功能發(fā)送給NCI，驅(qū)動6 個滑塊運動。

在實際控制過程中，系統(tǒng)通電之初并不知道動平臺當(dāng)前的位姿，只能通過導(dǎo)軌上的絕對值光柵尺反饋得到滑塊的位置值，此時需要通過正解算法根據(jù)滑塊位置計算出當(dāng)前動平臺的位姿，并將正解值賦給NCI 的6 個虛擬軸X、Y、Z、A、B、C。在得到位姿后，可以在PLC 程序中對NCI 通道中的虛擬軸X、Y、Z、A、B、C進行控制，包括點動、絕對和相對定位控制等。運動過程中NCI 會自動按照樣條曲線規(guī)劃出運動路徑，并在每個NC 周期計算出下一個目標(biāo)位姿，這個值作為C++反解模塊的輸入量，經(jīng)過計算就能夠?qū)崟r地輸出每個滑塊的移動量。為了讓伺服驅(qū)動器能夠根據(jù)每個周期給定的移動量實時驅(qū)動6 個伺服電機運動，這里用到了NC 信號發(fā)生器的外部給定功能。通常TwinCATNC 的設(shè)定位置、速度、加速度都是由NC 信號發(fā)生器產(chǎn)生的，在每個NC 周期會生成一套設(shè)定數(shù)據(jù)SetPoint。如果驅(qū)動器工作在位置模式，SetPoint 中的位置信號就會換算后發(fā)送給驅(qū)動器。但在一些特殊情況下，當(dāng)運動關(guān)系較為復(fù)雜時，如電機轉(zhuǎn)動的距離與工件實際運動反饋回的位置為非線性關(guān)系時，那么就需要用戶通過自己的算法來給定每個NC 周期的目標(biāo)位置。因此，上述的反解算法實際上就作為PLC 程序中使用的一個獨立的SetPoint 發(fā)生器，取代了NC 位置發(fā)生器的功能，在每個NC 周期將目標(biāo)位置數(shù)據(jù)發(fā)送給了伺服驅(qū)動器。得益于NCI 強大的路徑規(guī)劃能力，軌跡上位置的速度和加速度也都是連續(xù)的，這就保證了各滑塊運動過程的連續(xù)性。軟件框圖如圖7 所示。

圖7 控制軟件設(shè)計框圖

并聯(lián)機構(gòu)的上位機軟件運行于集成控制計算機，如圖8 所示，采用Qt C/C++IDE 開發(fā)，可實現(xiàn)上述控制功能，通過ADS 通信接口與運動控制器的TwinCAT3 軟件進行數(shù)據(jù)交換。

圖8 PC 端上位機控制界面

3 技術(shù)驗證

為了驗證本研究所設(shè)計的控制系統(tǒng)的正確性，使用激光跟蹤儀對動平臺的定位精度和重復(fù)精度進行測量。測量時環(huán)境溫度為25 ℃，相對濕度為30%。分別控制動平臺沿X、Y、Z、α、β、γ間隔一定步距（角度）平移和旋轉(zhuǎn)，測得的數(shù)據(jù)見表1。由測量結(jié)果可知，6-PUS 并聯(lián)機構(gòu)動平臺的定位精度和重復(fù)定位精度滿足系統(tǒng)設(shè)計要求。

表1 精度測試數(shù)據(jù)

對運動過程進行跟蹤，在TwinCAT3 軟件中抓取各滑塊的預(yù)定軌跡和實際運動軌跡，如圖9 所示。由圖9可以看到給定位置與實際位置曲線基本重合，這表明動平臺在實際運動過程中能夠依照預(yù)定的軌跡運動，保證了運動過程的穩(wěn)定可靠。

圖9 各滑塊實時運動曲線

設(shè)備的實物圖如圖10 所示，經(jīng)實驗驗證，文中方法在算法正確性、計算精度和執(zhí)行效率、各滑塊同步效果以及動平臺定位精度等各項性能指標(biāo)上完全滿足提出的技術(shù)要求。

圖10 直升機升力部件裝配設(shè)備實物圖

4 結(jié)論

探索6-PUS 并聯(lián)機構(gòu)的運動控制方法需要研究適合于控制系統(tǒng)實際控制需要的正、反解算法。通過牛頓迭代法求解位置正解的方法，計算時間短、求解效率高、唯一可達，滿足實時控制的要求。然后通過TwinCAT3軟件的IEC61131-3 和C++的混合編程方式，將C++編寫的正、反解算法封裝成模塊供PLC 程序調(diào)用，實現(xiàn)了同一控制系統(tǒng)下復(fù)雜算法與運動控制程序的結(jié)合。最后建立動平臺位姿的NCI 虛擬軸，通過控制虛擬軸運動和NCI 的路徑規(guī)劃能力，在每個NC 周期將目標(biāo)位姿解算出每個滑塊的移動量，由PLC 程序的外部給定功能給到各驅(qū)動器，并驅(qū)動各滑塊的實軸完成同步運動。對于類似結(jié)構(gòu)的并聯(lián)機構(gòu)的控制系統(tǒng)設(shè)計提供了借鑒和參考。