基于多項式插值的倒料機構(gòu)協(xié)同運動規(guī)劃方法

沈偉民，周岳松，張英杰，俞成蛟

(上海航天化工應(yīng)用研究所，浙江 湖州 313000)

火箭是國防工業(yè)的基礎(chǔ)產(chǎn)品之一，固體發(fā)動機作為火箭提供動力的核心部件，其質(zhì)量好壞決定了火箭的性能[1-4]。在火箭發(fā)動機生產(chǎn)過程中，需要對氧化劑進行轉(zhuǎn)運、裝填。傳統(tǒng)的發(fā)動機氧化劑裝填主要是由人工來實現(xiàn)的，效率低下。因為氧化劑具有易燃易爆的化學(xué)性質(zhì)，所以轉(zhuǎn)運過程中有防爆、防潮、環(huán)境清潔等要求，物料的轉(zhuǎn)運比較困難，對人身安全也帶來了巨大隱患。國外主要發(fā)達國家的發(fā)動機倒料機構(gòu)設(shè)備已具備很高的自動化、信息化和模塊化水平。國內(nèi)的設(shè)備相對落后，自動化水平較低，存在定位精度低，裝填效率低等問題，為提高生產(chǎn)效率和裝填精度，對一臺四自由度跟隨裝填倒料機構(gòu)的協(xié)同運動進行了規(guī)劃[5-6]。通過對原料桶出料口進行矢量建模、建立末端出料口的運動方程。根據(jù)目標軌跡對每個自由度驅(qū)動電機進行五次多項式插值規(guī)劃，通過翻轉(zhuǎn)、升降、水平移動和旋轉(zhuǎn)自由度驅(qū)動電機的協(xié)同運動確保氧化劑原料取料倒料操作的正確性和精準性。仿真分析結(jié)果表明倒料機構(gòu)能夠滿足裝藥精度要求，對設(shè)備的沖擊較小。

1 氧化劑原料倒料機構(gòu)結(jié)構(gòu)設(shè)計及工作原理

氧化劑倒料機構(gòu)用于火箭發(fā)動機生產(chǎn)線上發(fā)動機氧化劑裝填工作。需要完成氧化劑藥筒的抓取以及隨動裝藥工作。其具有X向、Y向直線運動和旋轉(zhuǎn)、翻轉(zhuǎn)回轉(zhuǎn)運動四個自由度。倒料機構(gòu)結(jié)構(gòu)的三維模型如圖1所示，旋轉(zhuǎn)機構(gòu)齒輪傳動結(jié)構(gòu)如圖2所示。

設(shè)備的X向移動是由外部軸驅(qū)動完成，伺服電機經(jīng)減速器與滾珠絲杠連接完成驅(qū)動與傳動，導(dǎo)軌滑塊機構(gòu)實現(xiàn)導(dǎo)向。設(shè)備的Y向運動同樣由固定于旋轉(zhuǎn)軸頂部的伺服電機和滾珠絲杠通過連接擺動橫梁實現(xiàn)。設(shè)備的旋轉(zhuǎn)運動是由電機減速器驅(qū)動，齒輪嚙合傳動完成，從動齒輪與旋轉(zhuǎn)軸底部相連，傳動精確可靠。翻轉(zhuǎn)運動通過電機減速器驅(qū)動，經(jīng)齒輪齒條的傳遞至翻轉(zhuǎn)機械手，從而實現(xiàn)翻轉(zhuǎn)操作。

機構(gòu)的剛度和精度是設(shè)計的關(guān)鍵指標，針對以上指標設(shè)計的倒料機構(gòu)具有以下結(jié)構(gòu)特點：

1)X向移動機構(gòu)為外部軸形式，剛度大、運動空間大，有利用X運動的快速實現(xiàn)，提高運動精度。

2) 旋轉(zhuǎn)和翻轉(zhuǎn)機構(gòu)采用低背隙的間隙機構(gòu)和角度編碼器實現(xiàn)位置雙閉環(huán)控制，提高運動精度以及平穩(wěn)性。

3) 設(shè)備四自由度之間采用串聯(lián)結(jié)構(gòu)形式，每個自由度都具有獨立性、便于控制。

氧化劑倒料機構(gòu)的工作原理：四自由度倒料機構(gòu)外部軸與生產(chǎn)線體平行放置。四自由度機械手先抓取氧化劑藥筒，放置生產(chǎn)線發(fā)動機裝藥口上端，初始位姿及各自由度參數(shù)可有位置及角度編碼器測量。裝藥過程中為保證裝藥質(zhì)量和精度，通過控制各自由度運動參數(shù)保證裝藥速率相同，出藥口位置高度不變，且隨發(fā)動機運動。

2 氧化劑原料倒料點位置建模

為了精確控制倒料機構(gòu)的倒料精度和質(zhì)量，必須對其建立運動方程來準確描述其空間位姿。常用的機器人運動描述采用D-H矩陣法和矢量法。由于本設(shè)備具有四個自由度，翻轉(zhuǎn)機構(gòu)的自由度參數(shù)已確定(為了保證倒料均勻，旋轉(zhuǎn)角速度滿足一定的變化規(guī)律)自由度較少，容易求解，所以采用向量法[7-8]。

裝藥時機械手需要夾持氧化劑藥筒，抓取藥筒示意圖如圖3。

Db和Hb分別為氧化劑料桶直徑和高度，D為氧化劑出料位置，yi為夾手夾持點與料筒底部距離。

為保證氧化劑原料桶倒料口的空間位置滿足目標軌跡路徑，并盡量減小設(shè)備運動過程中的沖擊，倒料機構(gòu)X軸(水平移動)、Y軸(豎直運動)、旋轉(zhuǎn)和翻轉(zhuǎn)等自由度需要進行協(xié)同運動，下面對氧化劑原料倒料點位置進行建模，得到倒料過程中多自由度聯(lián)動需要滿足的運動關(guān)系，倒料機構(gòu)的位置矢量關(guān)系在X-Z、X-Y方向的投影關(guān)系如圖4、圖5所示。

(1)

(2)

(3)

(4)

(5)

(6)

(7)

由以上公式可以得出原料口出料點的運動學(xué)方程：

Px=xi-Pri1cosθi-Pbi1sinθi=

xi-Lcosθi-Pbisinξicosφi

(8)

Py=Pr1sinθi-Pbicosθi=Lsinθi-Pbicosξicosφi

(9)

(10)

為了保證倒料的質(zhì)量和效率，在生產(chǎn)之前對設(shè)備的目標軌跡進行了設(shè)計，并通過激光跟蹤儀記錄了目標末端軌跡S0。則X、Y、Z方向的軌跡方程可表示為S0x(x，y，θ，φ)、S0y(x，y，θ，φ)、S0z(x，y，θ，φ)。

3 電機運動軌跡規(guī)劃

軌跡規(guī)劃是軌跡跟蹤控制的基礎(chǔ)，對工業(yè)機器人的工作效率、平穩(wěn)運行、降低能耗等均具有重要意義。通過氧化劑原料倒料點位置模型，可得到不同時刻倒料機構(gòu)各自由度的位移，為了保證倒料過程的穩(wěn)定性，防止氧化劑原料出現(xiàn)倒料過快、原料外倒現(xiàn)象，采用柔順性能較好和運算效率較高的五次多項式運動插補算法對倒料機構(gòu)各自由度電機的角位移軌跡進行規(guī)劃，以避免驅(qū)動電機在取料和倒料過程中剛性沖擊和柔性沖擊過大。確定一個五次多項式插值函數(shù)需要有6個約束條件[9-11]。五次插值函數(shù)形式如下

S(t)=at5+bt4+ct3+dt2+et+f

(11)

以首末兩位置處位移、速度、加速度為約束條件，其形式如下：

S(ti)=Si，S(tj)=Sj

S′(ti)=vi，S′(tj)=vj

S″(ti)=αi，S″(tj)=αj

(12)

由上兩式可以確定求解矩陣：

(13)

其中，t∈[ti，tj]，a，b，c，d，e，f為插補軌跡因子，可通過起點位置和終點位置的邊界條件得到，S1(t)、S2(t)、S3(t)和S4(t)分別為翻轉(zhuǎn)運動、升降運動、水平移動和旋轉(zhuǎn)運動電機角位移。

由翻轉(zhuǎn)機構(gòu)結(jié)構(gòu)可知，驅(qū)動電機運動經(jīng)由行星減速器和齒輪齒條的傳遞至翻轉(zhuǎn)機械手，從而實現(xiàn)翻轉(zhuǎn)操作，因而翻轉(zhuǎn)運動電機角位移的軌S1(t)為：

(14)

其中，i1為翻轉(zhuǎn)機構(gòu)行星減速器減速比。

由Y軸移動結(jié)構(gòu)和水平移動機構(gòu)可知，驅(qū)動電機的旋轉(zhuǎn)運動經(jīng)過減速器和滾珠絲杠轉(zhuǎn)化為升降柱的直線移動，升降機構(gòu)的電機角位移S2(t)和水平移動機構(gòu)電機角位移S3(t)為：

(15)

(16)

其中：i2和i3分別為升降機構(gòu)和水平移動機構(gòu)行星減速器減速比；d2和d3為升降機構(gòu)和水平移動機構(gòu)滾珠絲杠導(dǎo)程。

旋轉(zhuǎn)自由度采用伺服電機驅(qū)動齒輪傳動的方式實現(xiàn)旋轉(zhuǎn)運動，設(shè)i4為齒輪減速比，則驅(qū)動電機角位移S4(t)的表達式為

(17)

4 仿真驗證

首先在Simulink建立了系統(tǒng)規(guī)劃的程序框圖，如圖6所示。

程序框圖的原理為：對目標軌跡通過運動學(xué)求解，得到驅(qū)動電機位移，通過五次軌跡規(guī)劃得到實際的各電機驅(qū)動位移，進而求出設(shè)備實際軌跡。

運行程序，可得到每個電機的速度、加速度曲線及末端位置的X、Y、Z方向上的軌跡曲線。如圖7、圖8、圖9所示。

通過速度、加速度及末端位移曲線可知：通過協(xié)同規(guī)劃之后，設(shè)備運動的速度最大為0.18 m/s，角速度為6 rad/s，最大加速度為0.06 m/s2，最大角加速度0.58 rad/s2，運動平緩，沖擊較小。通過末端軌跡對比可知運動軌跡與目標軌跡之間誤差較小，最大處為4.23 mm，在允許誤差范圍之內(nèi)。

基于插值多項式建立的軌跡規(guī)劃方法，誤差來源主要由兩部分原因。一是該方法能夠準確設(shè)定確定位置處各電機的運動參數(shù)，中間位置的確定是基于確定位置點的運動參數(shù)插值模擬出來的，與實際的位置有一定偏差。二是串聯(lián)四自由度機構(gòu)的累計誤差造成實際位置與理想位置有一定誤差。這種誤差與確定位置點的個數(shù)有關(guān)，已知位置點個數(shù)越多，誤差越小。在無引入一些具體算法(如最小誤差控制)時跟隨誤差較大，在大行程跟隨中誤差在3 mm左右，引入具體算法后誤差較小，小于1 mm[12-13]。本研究的大行程軌跡跟隨沒有采取特殊跟隨算法最大誤差為3.24 mm，與國內(nèi)水平相比，效果相當。

5 結(jié)論

針對火箭發(fā)動機氧化劑裝填翻轉(zhuǎn)設(shè)備跟隨裝填位置精度較差的問題，提出一種基于多項式插值的協(xié)同運動規(guī)劃方法。先運用空間矢量疊加法建立原料筒出料口的矢量模型，求出出料口的運動方程，測定了目標軌跡，然后利用柔順的五次多項式軌跡規(guī)劃方法規(guī)劃電機的驅(qū)動角位移，最后通過Simulink對建立的模型進行仿真分析，仿真結(jié)果表明該種方法可保證氧化劑原料落點位置處于不同接料目標區(qū)域內(nèi)，實現(xiàn)對氧化劑原料倒料過程的柔順及精準控制。