基于DirectX的六維控制器虛擬仿真

張彥斐，陳 超，宮金良

(山東理工大學 機械工程學院, 山東 淄博 255049)

并聯機構具有剛度大、精度高等諸多優點，目前已經逐漸應用到各個領域，包括并聯機床、微操作機器人、誤差補償器、多維控制器等。并聯機構的運動學建模是控制系統設計的前提條件，相應建模方法較多，很多學者對此進行了深入研究[1-3]。陳宇航等[4]基于旋量運算得到子閉環中任意兩構件的相對速度和加速度，解決了含有子閉環的并聯機構動學建模問題。耿明超等[5]提出一種擬Newton法，提高了并聯機構運動學正解的計算效率，并有效提高了實時性。孫堅等[6]借助Matlab中SimMechanics模塊的系統動態建模功能，對Stewart平臺進行了運動學和動力學分析，避免了復雜的建模過程。另外，基于ADAMS軟件可以進行虛擬樣機的試驗和測試，也能夠有效簡化并聯機構的分析方法[7-8]。如張靜等[9]應用Inventor軟件建立幾何模型，以Parasolid格式導入ADAMS環境，對3-UPS/UP并聯機構進行了運動學仿真;賈小剛等[10]結合ADAMS與Pro/E軟件對并聯機構的虛擬樣機進行了運動學分析。

本文提出一種基于宏動3-URS并聯機構的六維控制器，建立相應的運動學模型，并利用DirectX技術和四元數表示的坐標系旋轉變換，進行六維控制器的虛擬仿真試驗。

1 3-URS并聯機構

圖1所示為一種基于宏動3-URS并聯機構，利用光電旋轉編碼器測量角位移的六維控制器。機構利用3條完全相同且呈中心對稱分布的支鏈，將靜平臺和動平臺連接在一起。6套驅動電機分成3組安裝在靜平臺上，分別為3個萬向鉸呈正交布置的兩軸提供動力輸入。六維控制器工作時，在力和力矩的作用下，動平臺的位姿將會發生改變，各關節量也會發生相應的變化。光電旋轉編碼器輸入改變的各關節變量，輸出經過濾波、電平轉換等調理之后的差分電壓模擬量信號，再通過數據采集卡將其倍頻處理后轉換成供計算機分析處理的數字量信號。計算機把采集的各關節變量，利用運動學模型及控制算法進行解算，控制虛擬機器人實現關聯運動和模擬動態仿真。

1.動平臺；2.上連桿；3.光電編碼器；4.下連桿；5.萬向鉸；6.靜平臺；7.力矩電機；8.安裝基座；9.轉動副；10.球鉸圖1 3-URS并聯機構三維模型Fig.1 Three-dimensional model of 3-URS parallel mechanism

2 基于D-H方法的運動學建模

為便于建立3-URS并聯機構的數學模型，將圖1所示的三維模型進行簡化處理，得到圖2所示的3-URS并聯機構原理簡圖，其中Ai(i=1,2,3)為萬向鉸兩垂直交錯軸的交點，Bi為每條支鏈末端球鉸的中心點。在靜平臺中心固結參考坐標系{O}，Y軸正方向通過A1，Z軸垂直向上，X軸正方向由右手螺旋法則確定。在動平臺中心固結動坐標系{O1}，初始位置各坐標軸的方向與參考坐標系一致。αi為繞X0軸的轉角，βi為繞Z0軸的轉角，γi為PiAi與PiBi之間的夾角，δ為PiAi與Z1軸之間的夾角，a為靜平臺的邊長，b為動平臺的邊長。

圖2 3-URS并聯機構原理簡圖Fig.2 Schematic diagram of 3-URS parallel mechanism

2.1 動平臺運動學正解

采用D-H矩陣變換法對并聯機構進行運動學建模。表1中給出了各連桿的D-H參數。

表1 D-H參數表
Tab.1 D-H Parameters

序號θdaα1π2+2i-1()3π0a3-π22-π2+βi()00π23-arctan(tanαicosβi)-l1cosδl1sinδπ24π2-γi-δ()0l20

相鄰連桿之間的位姿關系可用坐標旋轉和平移的方法建立聯系，變換矩陣Ai為

式中，Sθi=sinθi，Cθi=cosθi。Bi點相對于參考坐標系的變換矩陣為

Ti=A1iA2iA3iA4i

(1)

在矩陣Ti中用位置矢量Bi=[pxipyipzi]T表示球鉸中心Bi的位置，用旋轉矩陣R=[noa]表示末端連桿的空間姿態。將表1中各參數帶入式(1)，求得Bi的坐標

(2)

式中：

為了便于在編程中實現六維控制器對虛擬仿真的控制，采用Z-Y-X型動態歐拉角來描述上平臺相對于參考坐標系的姿態。其中旋轉矩陣R=[noa]，可通過3條支鏈末端球鉸中心Bi的空間坐標值進行求解。各角的反正切表達式為

(3)

式中，

上平臺中心O1點的坐標為

(4)

式(3)、式(4)即為并聯機構位置正解，等號右邊是用結構參數和關節變量表示的解析表達式來確定動平臺的位姿。當給定各關節變量的變化范圍時，便可求得3-URS并聯機構的工作空間。

2.2 上連桿運動學正解

式(2)求得Bi的坐標即為上連桿的位置坐標。上連桿的姿態采用Z-Y-X型動態歐拉角進行描述。在齊次變換矩陣Ti中的旋轉矩陣R=[noa]，其各單位方向矢量在參考坐標系上的分量為動坐標系各坐標軸的方向余弦。向量n、o、a的表達式分別如下：

式中：

L1=sinθitanαicosβi-cosθisinβi

L2=cosθitanαicosβi+sinθisinβi

R1=cosθicosβi

R2=sinθicosβi

Q=sinβicosβitanαi

上連桿Z-Y-X型動態歐拉角的反正切表達式為

(5)

式(2)、式(5)即為上連桿的空間姿態表達式。其空間位姿的正確性是3-URS并機構能否實現在虛擬環境中關聯運動的關鍵。

3 MATLAB仿真實例

(a)位形1 (b)位形2 (c)位形3 (d)位形4圖3 不同位形參數下的機構位形簡圖Fig.3 Pose examples with different input variables

表2 機構正解
Tab.2 Forward kinematic of the mechanism

[α1,α2,α3,β1,β2,β3,γ1,γ2,γ3][x,y,z,α,β,γ][0,0,0,-2.5,-2.5,-2.5,50,50,50][0,0,144.631,0,0,0][3.1,0.3,-23.2,-15.8,11.7,-3.2,57.8,71.3,64.6][-52.498,3.612,183.769,0.478,0.026,-0.110][-6.3,-9.9,20.1,10.4,-19.8,-4.3,67.7,49.8,59.2][53.945,0.531,168.535,-0.107,0.154,-0.024][18.3,-18.1,1.5,7.5,6.5,-27.5,70.7,70.2,52.1][1.897,73.049,181.047,-0.031,-0.010,-0.105]

采用數值驗證的方法來驗證運動學正解的可靠性。根據運動學正解，求得動平臺的位姿參數，然后將其與表3三維建模軟件的標定值進行比較，結果表明誤差均≤5×10-3。由此驗證了3-URS并聯機構運動學正解的正確性以及結構設計的合理性和可行性。

表3 機構位姿標定及誤差
Tab.3 Error and pose calibration

[x,y,z,α,β,γ]δ[0,0,144.631,0,0,0]≤5×10-3[-52.497,3.610,183.773,0.478,0.025,-0.106]≤5×10-3[53.945,0.534,168.530,-0.107,0.152,-0.025]≤5×10-3[1.894,73.052,181.050,-0.031,-0.012,-0.107]≤5×10-3

4 虛擬仿真系統

六維控制器的末端將外部受力與力矩直接和輸出的位移增量和轉動增量建立關系，然后再作用于各關聯構件反推出其位姿。數據采集卡即時采集光電旋轉編碼器監測的各關節變量，在VS中利用DirectX技術把采集到的各關節變量，依據運動學正解模型進行處理，計算出各關聯構件的空間位姿，實現3-URS并聯機構在虛擬三維場景中關聯運動并進行模擬仿真分析，控制系統流程如圖4所示。

圖4 控制系統流程圖Fig.4 Flow chart of the control system

以上連桿為例進行介紹。根據各關節編碼器所采集的關節變量，依據式(2)求得球鉸中心Bi的坐標，依據式(5)求得上連桿的動態歐拉角。由于在DirectX中旋轉變換是以世界坐標系進行變換，而求得的上連桿歐拉角是以其動坐標系進行變換。所以為了解決利用DirectX技術重繪上連桿的空間位姿問題，需要求得上連桿動坐標系在各次旋轉變換中的空間位姿。這樣就把繞上連桿動坐標系的旋轉變換轉換為繞世界坐標系中的特定軸的旋轉變換。

點在左手直角坐標系中繞過原點任意軸r=(rx，ry，rz)旋轉θ角的旋轉變換矩陣為

Rot(r,θ)=

式中，

初始情況下，上連桿的動坐標系與世界坐標系完全重合。在世界坐標系的原點位置，依據上式旋轉上連桿動坐標系的3個坐標軸，變換到所需的姿態后，平移到Bi點，再進行渲染操作。主要步驟如下：

(1)繞動坐標系的Z軸旋轉α角。此時動坐標系的X軸和Y軸在世界坐標系下的矢量分別為[cosα -sinα 0]T、[sinα cosα 0]T。

(2)繞動坐標系的Y軸旋轉β角。局部坐標系的X軸在世界坐標系下的矢量為[cosαcosβ-sinαcosβsinβ]T。

(3)繞動坐標系的X軸旋轉γ角。此時得到的坐標系即所需的姿態坐標系。

在編程實現上連桿的姿態變化時，若中間旋轉變換的過程采用四元數Q(x,y,z,w)=[sin(w/2)x, sin(w/2)y, sin(w/2)z, cos(w/2)](其中[x，y，z]為旋轉軸，w為旋轉角度)來表示坐標系的旋轉變換，則可以有效避免物體在旋轉變換的過程中出現歐拉角萬向節死鎖、矩陣數據處理量大、物體旋轉變換出現不連續的問題。因為四元數具有不論剛體處于什么空間位姿都不會退化的優點，并且參數的范圍不受限制，方程組線性化程度高，無奇異，只涉及代數運算，所以避免了大量三角函數運算。由四元數到旋轉矩陣的變換公式為

Rot(r,θ)=

式中，

四元數將三維空間的旋轉概念擴展到四維空間，這為表示和處理三維空間中點的旋轉變換帶來了極大方便。在程序中若廣泛用四元數替換矩陣變換，不僅能節省內存資源，提高數據運算速度，同時也能降低計算處理成本。若需姿態變換矩陣，可依據上式把四元數轉換成姿態變換矩陣。圖5為3-URS并聯機構虛擬仿真實例效果。

(a)效果1 (b)效果2

(c)效果3 (d)效果4圖5 虛擬仿真系統運行效果圖Fig.5 Results of the virtual simulation system

5 結論

1) 提出了一種基于宏動3-URS并聯機構，利用光電旋轉增量式編碼器測量角位移的六維控制器設計方案。此并聯機構利用3條完全相同且呈中心對稱分布的URS支鏈，將靜平臺和動平臺連接在一起，充分集成了并聯機構、光電旋轉增量式編碼器、萬向鉸等的優點，為六維控制器的結構設計提供了一種新思路。

2) 已知六維控制器的各關節變量，對并聯機構的正向運動學進行了封閉式完全耦合分析并利用MATLAB對數學模型進行了有效驗證，從而為實時測量動平臺的位姿提供了有力保證，同時也為虛擬環境中并聯機器人的運動仿真分析提供了必要的理論依據。

3) 引入四元數表示坐標系旋轉變換并進行正交規范化處理，基于DirectX技術構建了虛擬仿真系統，實現了六維控制器對虛擬機器人的實時控制，為機器人運動學驗證提供了實時檢測平臺。