輕型飛行模擬器并聯(lián)機(jī)構(gòu)約束檢驗(yàn)算法的研究

泰州職業(yè)技術(shù)學(xué)院 陳 靜上海飛機(jī)設(shè)計(jì)研究院 李 凱

在實(shí)際應(yīng)用中，輕型飛行模擬器的邊置式座艙結(jié)構(gòu)的6根伸縮桿在其行程范圍內(nèi)隨意運(yùn)動(dòng)時(shí)，僅存在動(dòng)平臺(tái)座艙干涉和等效復(fù)合球鉸擺角約束的情況。要設(shè)計(jì)出滿足各項(xiàng)約束條件的輕型飛行模擬器控制算法，首先要解決的問(wèn)題就是能夠?qū)Ω黜?xiàng)約束進(jìn)行檢驗(yàn)，包括桿長(zhǎng)限制、擺角約束和座艙干涉，且檢驗(yàn)算法越簡(jiǎn)單越好[1]。

1 并聯(lián)機(jī)構(gòu)連桿干涉算法

1.1 干涉檢驗(yàn)算法

目前求解連桿干涉的檢驗(yàn)算法一般有3種[2]，本文以連桿l1和l2為例進(jìn)行討論。通常情況下，連桿是有一定尺寸的，為了討論方便，假設(shè)連桿都是圓柱形，分別為C1、C2，其半徑為 r1、r2。

（1）算法1。

該算法通過(guò)計(jì)算2線段l1和l2之間的距離d來(lái)判斷C1和C2是否干涉。

令l1=A1-B1，l2=A2-B2，則l1和l2所在直線的最短距離為

設(shè)M1和M2為公垂線與l1和l2的交點(diǎn)，則有A1M1=u1A1B1，A2M2=u2A2B2，其中

如果d＞r1+r2，則兩連桿不干涉；如果d≤r1+r2且d=d12，則兩連桿干涉；如果d≤r1+r2且d≠d12，則從d的定義上不能直接判斷兩連桿是否干涉，需要通過(guò)計(jì)算公垂線垂足的位置來(lái)加以判斷。

（2）算法2。

定 義 n2為 l2的單位向量，v2、ω2是兩個(gè)與n2互相垂直相交的向量?？紤]l2上一點(diǎn)M2，則A2M2=u2A2B2。M為M2所在圓柱體截面上一點(diǎn)，則M2M=r2cosθv2+r2cosθw2。因此M點(diǎn)在靜坐標(biāo)系內(nèi)的坐標(biāo)為

取 l1上一點(diǎn)M1，則A1M1=uA1B1，如果|M1M|＞r1且u1，u1∈[0，1]，θ∈[0，2π]，則兩連桿不干涉；如果|M1M|≤ r1且A1M·A1B1≥ 0，B1M·A1B1≥ 0， 則兩連桿干涉。

該算法是一個(gè)全面干涉檢驗(yàn)算法，因而比算法1更加完整。

（3）算法3。

取M為l1所在圓柱體上一點(diǎn)，有

第一個(gè)不等式表示M點(diǎn)到l1的距離小于圓柱體半徑r1，第二個(gè)不等式表示M在圓柱體C1范圍內(nèi)。可以用相似的3個(gè)不等式的形式表示M在圓柱體C2范圍內(nèi)。如果存在一個(gè)點(diǎn)M都滿足這樣6個(gè)不等式，則連桿干涉，否則不干涉。這個(gè)算法與算法2相似，提供了一種完整的干涉檢驗(yàn)，但需要解6個(gè)不等式而不是算法2的3個(gè)不等式。

1.2 干涉算法及分析

以算法1為基礎(chǔ)，在d≤r1+r2且d≠d12的情況下，通過(guò)計(jì)算公垂線垂足的位置來(lái)進(jìn)行干涉檢驗(yàn)。設(shè) M1、M2分別為公垂線在兩連桿軸線上的垂足。

如果u1?[0，1]，u2?[0，1]，即兩個(gè)垂足都在連桿軸線外部，建立如下公式

如果，則 d 由式（7）求得。

如果，則 d 由式（8）求得。

兩連桿如干涉，則應(yīng)首先在最小距離方向上發(fā)生，因此應(yīng)求出兩連桿半徑在最小距離方向上的投影長(zhǎng)度。計(jì)算最小距離時(shí)，如果取式（7），則最小距離方向?yàn)锳1M2；如果取式（8），則最小距離方向?yàn)锳2M1；如果取式（9），則最小距離方向?yàn)锳1A2。而點(diǎn)Mi（i=1，2，...，6）在靜坐標(biāo)系OB-XBYBZB中的坐標(biāo)為。

這樣如果用N1表示兩連桿l1和l2的最小距離方向，用θ1和θ2分別表示N1與l1和l2所夾銳角，則兩連桿在最小距離方向上的投影長(zhǎng)度為：

綜上所述，連桿l1和l2干涉時(shí)應(yīng)滿足條件d≤r'。

相鄰連桿干涉檢測(cè)算法的步驟如下。

步驟1：計(jì)算靜坐標(biāo)系OB-XBYBZB下連桿鉸鏈點(diǎn)坐標(biāo)及連桿向量。

步驟2：計(jì)算相鄰兩桿間的公垂線長(zhǎng)度d。如果d＞r1+r2，則兩連桿不干涉且算法結(jié)束，否則繼續(xù)下一步。

步驟3：計(jì)算 u1、u2。

（1）如果，，則兩連桿發(fā)生干涉，算法結(jié)束；

（2）如果，則計(jì)算公式（7），并計(jì)算點(diǎn)M1的坐標(biāo)及向量N1=A2M1。

（3）如果，則計(jì)算公式（8），并

計(jì)算點(diǎn)M2的坐標(biāo)及向量N1=A1M2。

步驟4：如果，則計(jì)算 k1、k2。

（1）若則 計(jì) 算公式（9），且N1=A1A2；

（2）若，則計(jì)算公式（7），并計(jì)算點(diǎn)M2的坐標(biāo)及向量N1=A1M2。

（3）若則計(jì)算公式（8），并計(jì)算M1的坐標(biāo)及向量N1=A2M1；

步驟5：根據(jù)公式（10）計(jì)算兩連桿在最小距離方向上的投影長(zhǎng)度r。

步驟6：如果d≤r'，兩連桿發(fā)生干涉；否則，兩連桿不發(fā)生干涉。

步驟7：算法結(jié)束。

該算法通過(guò)計(jì)算u1、u2和k1、k2，對(duì)公垂線與連桿軸線的交點(diǎn)位置進(jìn)行了判斷，進(jìn)而通過(guò)對(duì)兩連桿半徑在最小距離方向上的投影長(zhǎng)度的計(jì)算，可準(zhǔn)確地判別兩相鄰連桿在空間是否干涉。

需要指出的是，為了進(jìn)一步提高算法效率，在實(shí)際應(yīng)用中常常用r1+r2代替r，這樣就減少了計(jì)算M、N、θ的時(shí)間，而且同樣保證了兩桿不會(huì)干涉。

2 并聯(lián)機(jī)構(gòu)動(dòng)平臺(tái)座艙干涉檢驗(yàn)算法

輕型飛行模擬器采用邊置式座艙結(jié)構(gòu)，平臺(tái)座艙位于動(dòng)平面下方，如圖1所示，因而要考慮動(dòng)平臺(tái)座艙與各連桿之間的干涉問(wèn)題[3]。

圖1 動(dòng)平臺(tái)結(jié)構(gòu)示意圖Fig.1 Diagram of moving platform

2.1 干涉檢驗(yàn)算法

為了計(jì)算方便及提高算法效率，在實(shí)際應(yīng)用中采用一個(gè)直徑DL略小于平臺(tái)最大尺寸的空間圓柱體包絡(luò)座艙。研究座艙干涉檢驗(yàn)算法是為了避免座艙在運(yùn)動(dòng)時(shí)與連桿發(fā)生碰撞。當(dāng)座艙在空間運(yùn)動(dòng)時(shí)，計(jì)算動(dòng)平臺(tái)與各連桿最小距離問(wèn)題就轉(zhuǎn)化為計(jì)算圓柱體與各連桿最小距離問(wèn)題，如圖2所示。

圖2 平臺(tái)連桿干涉情況Fig.2 Interference situation of platform connecting rod

動(dòng)平臺(tái)是一個(gè)邊置式座艙，其下底面所在圓周的圓心為O'，半徑為r'，座艙高為h。Pi為連桿中心線與平臺(tái)下底面所在平面的交點(diǎn)。Mi點(diǎn)為O'P連線與底面圓周的交點(diǎn)。則Mi點(diǎn)到連桿中心線距離為平臺(tái)與連桿干涉時(shí)應(yīng)滿足條件di＜ D/2。

2.2 干涉算法及分析

座艙與連桿干涉算法步驟如下。

步驟1：計(jì)算靜坐標(biāo)系OB-XBYBZB下連桿鉸鏈點(diǎn)坐標(biāo)、座艙底面圓心坐標(biāo)O'及連桿向量；

步驟2：計(jì)算圓O'所在平面與連桿AiBi所在直線的交點(diǎn)Pi；

步驟3：計(jì)算O'P矢量，以及O'P與底面圓周的交點(diǎn)Mi的坐標(biāo)；

步驟4：計(jì)算Mi到連桿AiBi的距離di和垂足Ni；步驟5：如果di＜ D/2，則座艙與連桿AiBi干涉；否則，不干涉。

該算法通過(guò)計(jì)算座艙底面圓周與連桿的最短距離來(lái)判斷是否干涉，具有簡(jiǎn)單、高效的特點(diǎn)，同時(shí)計(jì)算出Mi、Ni的空間坐標(biāo)，為研究下文提出的“虛擬彈簧”作用力提供了便利。

3 動(dòng)平臺(tái)復(fù)合球鉸擺角檢測(cè)算法

實(shí)際應(yīng)用中，機(jī)座和支鏈之間常常通過(guò)虎克鉸鏈連接，動(dòng)平臺(tái)和支鏈之間常常通過(guò)球鉸連接。虎克鉸鏈的擺角限制問(wèn)題在輕型飛行模擬器機(jī)構(gòu)設(shè)計(jì)時(shí)就已經(jīng)解決，本文不再進(jìn)行研究。動(dòng)平臺(tái)復(fù)合球鉸的擺角是有限制的，本文給出其檢測(cè)方法。

3.1 干涉檢驗(yàn)算法

以連接于動(dòng)平臺(tái)Ai點(diǎn)的l1和l6為例討論。如圖3所示，將等效復(fù)合球鉸進(jìn)行空間簡(jiǎn)化。

平臺(tái)運(yùn)動(dòng)時(shí)，支承軸始終與動(dòng)坐標(biāo)軸ZP方向一致，因而在計(jì)算過(guò)程中可用座艙圓柱體中心矢量OAO'代替支承軸方向矢量進(jìn)行計(jì)算，則支承軸所在空間直線L0與球鉸中心線L1夾角為

圖3 復(fù)合球鉸擺角計(jì)算示意圖Fig.3 Swing angle calculation sketch of compound sphere joint

式中，n1,6為l1和l6所在平面的法向矢量

支承軸與球鉸的夾角有一個(gè)極限區(qū)域值，記為，則約束檢測(cè)條件為

3.2 干涉算法及分析

等效復(fù)合球鉸干涉算法步驟如下。

步驟1：計(jì)算靜坐標(biāo)系OB-XBYBZB下連桿鉸鏈點(diǎn)坐標(biāo)、座艙底面圓心坐標(biāo)O'及連桿向量l1、l6和平臺(tái)中心矢量OAO'；

步驟2：計(jì)算l1和l6的法向矢量n1,6；

步驟3：計(jì)算OAO'與n1,6的夾角θ1,6；

步驟4：判斷θ1,6是否在有效區(qū)域內(nèi)。如果在，則不干涉；否則，干涉。

該算法通過(guò)計(jì)算鉸鏈于一點(diǎn)的兩連桿矢量所在平面的法向矢量與球鉸支承軸矢量的夾角來(lái)判斷鉸鏈擺角是否有效，計(jì)算簡(jiǎn)單、可行。

4 結(jié)論

本文首先介紹了并聯(lián)機(jī)構(gòu)工程應(yīng)用中常用的Newton-Raphson迭代法，設(shè)計(jì)了位姿正解算法步驟；其次，列舉了連桿干涉的3種算法，并以算法1為基礎(chǔ)，設(shè)計(jì)了一種連桿干涉檢驗(yàn)算法；最后，分析研究了平臺(tái)座艙干涉算法以及擺角檢驗(yàn)算法，設(shè)計(jì)了算法步驟，這些為以后研究分析輕型飛行模擬器的工作空間提供了一定依據(jù)。

[1]楊灝泉，趙克定，吳盛林，等. 飛行模擬器六自由度運(yùn)動(dòng)系統(tǒng)的關(guān)鍵技術(shù)及研究現(xiàn)狀. 系統(tǒng)仿真學(xué)報(bào), 2002, 14(1)：84-87.

[2]Merlet J-P, Daney D. Legs interference chercking of parallel robots over a given workspace or trajectory//IEEE. Proceedings of the 2006 IEEE International Conference on Robotics and Automation,2006：605-619.

[3]周冰.Stewart平臺(tái)并聯(lián)機(jī)器人設(shè)計(jì)與干涉防護(hù)問(wèn)題研究[D].西安：西安交通大學(xué)， 2002.