張斌，陳建文，冷慧玲，祝沛楨

（核動(dòng)力運(yùn)行研究所，武漢 430200）

0 引言

核電機(jī)組的長期運(yùn)行會(huì)產(chǎn)生大量廢棄的燃料相關(guān)組件，廢棄的燃料相關(guān)組件存放于乏燃料池中，而乏燃料水池可用的有效空格架數(shù)量有限。為了最大限度地減小相關(guān)組件的貯存空間，研制并應(yīng)用水下立式縮容處理機(jī)器人對相關(guān)組件進(jìn)行縮容處理[1]?？s容機(jī)器人的液壓剪切機(jī)構(gòu)在水下剪切相關(guān)組件單棒，剪斷的單棒需要通過單棒轉(zhuǎn)移機(jī)械手進(jìn)行轉(zhuǎn)移和分類存儲(chǔ)。

相關(guān)組件具有一定的輻照劑量，縮容工藝需通過水屏蔽，在水下進(jìn)行縮容作業(yè)，單棒轉(zhuǎn)移機(jī)械手在操作單棒時(shí)會(huì)受流體作用力影響。單棒在水下的運(yùn)動(dòng)問題屬于圓柱繞流[2]問題，一方面繞流阻力會(huì)限制單棒的運(yùn)動(dòng)速度，另一方面單棒運(yùn)動(dòng)可能產(chǎn)生流致振動(dòng)[3]，會(huì)影響單棒轉(zhuǎn)運(yùn)的穩(wěn)定性和可靠性。

針對單棒轉(zhuǎn)移機(jī)械手在水下操作存在的問題，本文對單棒轉(zhuǎn)移機(jī)械手的末端軌跡規(guī)劃進(jìn)行研究并設(shè)計(jì)機(jī)械手控制系統(tǒng)確保機(jī)器人的平穩(wěn)運(yùn)動(dòng)，實(shí)現(xiàn)縮容工藝中單棒轉(zhuǎn)移機(jī)器手的穩(wěn)定搬運(yùn)和存儲(chǔ)。

1 系統(tǒng)結(jié)構(gòu)及參數(shù)

單棒轉(zhuǎn)移機(jī)械手是水下立式縮容處理機(jī)器人的組成部分之一。水下立式縮容處理機(jī)器人系統(tǒng)整體位于乏池水下約10 m水深環(huán)境中，其中水體溫度約為50 ℃。立式縮容處理機(jī)器人系統(tǒng)組成包括液壓剪切機(jī)構(gòu)、組件旋轉(zhuǎn)機(jī)構(gòu)、存儲(chǔ)容器傾斜機(jī)構(gòu)、組件連接柄轉(zhuǎn)移機(jī)械手和單棒轉(zhuǎn)移機(jī)械手等。液壓剪切機(jī)構(gòu)用于剪切相關(guān)組件單棒，其刀具位置可調(diào)整，組件旋轉(zhuǎn)機(jī)構(gòu)用于旋轉(zhuǎn)燃料組件，容器傾斜機(jī)構(gòu)用于傾斜存儲(chǔ)容器，可調(diào)整其傾斜角度，連接柄轉(zhuǎn)移機(jī)械手用于轉(zhuǎn)運(yùn)燃料組件上方連接柄，具有2個(gè)移動(dòng)自由度。單棒轉(zhuǎn)移機(jī)械手用于夾持并穩(wěn)定轉(zhuǎn)運(yùn)單棒，具有3個(gè)移動(dòng)自由度和1個(gè)夾持自由度。水下立式縮容處理機(jī)器人整體如圖1所示。

圖1 水下立式縮容處理機(jī)器人

單棒轉(zhuǎn)移機(jī)械手結(jié)構(gòu)如圖2所示。機(jī)械手共有4個(gè)關(guān)節(jié)，升降關(guān)節(jié)用于驅(qū)動(dòng)機(jī)械手整體在垂直方向的升降；旋轉(zhuǎn)關(guān)節(jié)用于驅(qū)動(dòng)機(jī)械手在水平面上旋轉(zhuǎn)，伸縮關(guān)節(jié)驅(qū)動(dòng)機(jī)械臂在水平面上進(jìn)行伸長和收縮，夾持關(guān)節(jié)為機(jī)械手爪，用于護(hù)持和夾持單棒。

圖2 單棒轉(zhuǎn)移機(jī)械手

4個(gè)關(guān)節(jié)均通過直流伺服電動(dòng)機(jī)驅(qū)動(dòng)，機(jī)械手升降關(guān)節(jié)、旋轉(zhuǎn)關(guān)節(jié)和伸縮關(guān)節(jié)的運(yùn)動(dòng)直接確定了其末端位置。各個(gè)關(guān)節(jié)的相關(guān)參數(shù)如表1所示。

表1 單棒轉(zhuǎn)移機(jī)械手關(guān)節(jié)參數(shù)

2 建模分析

為解決單棒轉(zhuǎn)移機(jī)械手在水下操作單棒的問題，需建立單棒轉(zhuǎn)移機(jī)械手的正、逆運(yùn)動(dòng)學(xué)模型和微分運(yùn)動(dòng)學(xué)模型，用于對機(jī)械手進(jìn)行末端軌跡規(guī)劃和運(yùn)動(dòng)控制。同時(shí)需建模分析水流對單棒運(yùn)動(dòng)造成的阻力和振動(dòng)影響，設(shè)計(jì)的控制器需在水流影響下保持穩(wěn)定性。

2.1 運(yùn)動(dòng)學(xué)模型

根據(jù)單棒轉(zhuǎn)移機(jī)械手的結(jié)構(gòu)參數(shù)，對其進(jìn)行運(yùn)動(dòng)學(xué)分析。夾爪末端坐標(biāo)用向量X表示，3個(gè)電動(dòng)機(jī)角度用向量q表示，采用幾何法，根據(jù)單棒轉(zhuǎn)移機(jī)械手的結(jié)構(gòu)特點(diǎn)進(jìn)行分析，則有X=T（q），其中：

2.2 微分運(yùn)動(dòng)學(xué)模型

將X=T（q）兩邊微分，單棒轉(zhuǎn)移機(jī)械手的微分運(yùn)動(dòng)學(xué)方程可寫成δX=J（q）δq，由T（q），可計(jì)算J（q）如下：

J（q）表示單棒轉(zhuǎn)移機(jī)械手操作空間與關(guān)節(jié)空間的速度轉(zhuǎn)換關(guān)系，為機(jī)械手的速度雅可比矩陣，J（q）T表示單棒轉(zhuǎn)移機(jī)械手操作空間與關(guān)節(jié)空間的力、力矩轉(zhuǎn)換關(guān)系，為機(jī)械手的力雅可比矩陣。

2.3 單棒繞流阻力模型

單棒轉(zhuǎn)移機(jī)械手夾持相關(guān)組件單棒在水下運(yùn)動(dòng)時(shí)，機(jī)械臂和單棒均會(huì)受到水體作用力的影響。機(jī)械臂結(jié)構(gòu)健壯，水體作用力對其干擾作用不大，而單棒整體呈細(xì)長棒狀，通過一個(gè)夾持點(diǎn)被機(jī)械臂夾持，受水流影響可能會(huì)產(chǎn)生彎曲或夾持脫落，需對單棒受水體作用力模型進(jìn)行分析。

單棒在乏池的運(yùn)動(dòng)過程屬于典型的繞流問題，水體與單棒的相對運(yùn)動(dòng)會(huì)產(chǎn)生繞流阻力，根據(jù)繞流公式，計(jì)算繞流阻力、繞流升力和流致振動(dòng)頻率：

式中：FD為繞流阻力；FL為繞流升力；f為繞流升力的脈動(dòng)頻率；CD為繞流阻力系數(shù)；CL為繞流升力系數(shù)；Sr為斯特勞哈數(shù)；CD、CL、Sr與雷諾數(shù)Re有關(guān)；ρ為水流密度；v為單棒相對水池的運(yùn)動(dòng)速度；d為單棒直徑；A為單棒在垂直于運(yùn)動(dòng)方向平面上的投影面積。

計(jì)算單棒運(yùn)動(dòng)時(shí)的雷諾數(shù)：

式中：μ為流體動(dòng)力黏性系數(shù)；L為流場特征長度。

3 軌跡規(guī)劃與控制

3.1 軌跡規(guī)劃

為使單棒轉(zhuǎn)移機(jī)械手在水下操作單棒時(shí)具有較好的穩(wěn)定性和運(yùn)動(dòng)平穩(wěn)性，在軌跡規(guī)劃時(shí)需考慮運(yùn)動(dòng)速度與水體繞流阻力的關(guān)系，選擇合適的運(yùn)動(dòng)速度，兼顧轉(zhuǎn)移效率和穩(wěn)定性。同時(shí)為保證夾持的可靠性，轉(zhuǎn)移軌跡的位置、速度、加速度應(yīng)連續(xù)且變化平緩。

貝塞爾曲線[4]軌跡一定經(jīng)過起始點(diǎn)P0和終點(diǎn)，軌跡處處連續(xù)，且其導(dǎo)數(shù)仍為貝塞爾曲線，用貝塞爾曲線規(guī)劃的軌跡，其位置、速度、加速度連續(xù)且變化平緩。采用貝塞爾曲線規(guī)劃運(yùn)動(dòng)軌跡，通過標(biāo)定與示教設(shè)置控制點(diǎn)，確保機(jī)械手運(yùn)動(dòng)軌跡符合要求。

設(shè)軌跡規(guī)劃時(shí)給定了n個(gè)控制點(diǎn)P0、P1、P2…Pn，其貝塞爾曲線的一般表達(dá)式可寫為

式中：CP0為初始狀態(tài)時(shí)機(jī)械臂末端坐標(biāo)；CP5為機(jī)械臂的目標(biāo)位置。根據(jù)各控制點(diǎn)，生成的機(jī)械臂末端軌跡如圖3 所示，機(jī)械臂升降、旋轉(zhuǎn)、伸縮關(guān)節(jié)的位置和速度規(guī)劃 如 圖4、圖5、圖6所示。

圖3 單棒轉(zhuǎn)移機(jī)械手臂末端運(yùn)動(dòng)軌跡

圖4 升降關(guān)節(jié)規(guī)劃位置和規(guī)劃速度

圖5 旋轉(zhuǎn)關(guān)節(jié)規(guī)劃位置和規(guī)劃速度

圖6 伸縮關(guān)節(jié)規(guī)劃位置和規(guī)劃速度

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

為避免繞流阻力對單棒轉(zhuǎn)移機(jī)械手臂的運(yùn)動(dòng)產(chǎn)生不利影響，采用前饋控制的思想，通過前饋補(bǔ)償繞流阻力[5]，在較大程度上消除機(jī)械手臂運(yùn)動(dòng)過程中的繞流阻力的影響。同時(shí)為提高控制系統(tǒng)抵抗外部干擾的能力，采用PD控制算法[6]對機(jī)械手臂末端位置進(jìn)行控制。

設(shè)計(jì)帶前饋控制的控制率u計(jì)算公式為

控制系統(tǒng)框圖如圖7所示。

圖7 控制系統(tǒng)框圖

4 仿真分析

為確保單棒轉(zhuǎn)移機(jī)械手軌跡規(guī)劃和控制算法的有效性，采用仿真分析對機(jī)械手的運(yùn)動(dòng)軌跡和控制算法進(jìn)行驗(yàn)證并優(yōu)化。

仿真計(jì)算平臺(tái)為Matlab 2019，采用Simulink完成規(guī)劃和控制算法，采用Simscape模塊進(jìn)行物理仿真和三維可視化驗(yàn)證。

4.1 仿真模型

搭建Simscape仿真模型如圖8所示，三維可視化效果如圖9所示，模型的輸入為各關(guān)節(jié)電動(dòng)機(jī)力/轉(zhuǎn)矩，模型輸出為各關(guān)節(jié)當(dāng)前位置和速度，且添加了外部繞流阻力和繞流升力作用。

圖8 Simscape單棒轉(zhuǎn)移機(jī)械手仿真模型

圖9 Simscape單棒轉(zhuǎn)移機(jī)械手三維可視化

根據(jù)Simscape模型，在Simulink中構(gòu)件控制系統(tǒng)如圖10所示。其中，PD位置控制器模塊的輸入為貝塞爾軌跡，輸出為各關(guān)節(jié)力/力矩。繞流力補(bǔ)償模塊根據(jù)規(guī)劃速度計(jì)算了繞流力對應(yīng)的力/力矩補(bǔ)償值，流體作用力計(jì)算模塊根據(jù)單棒運(yùn)動(dòng)速度計(jì)算了實(shí)時(shí)的繞流阻力和繞流升力，并將其作用于Simscape模型中。

圖10 Simulink單棒轉(zhuǎn)移機(jī)械手控制系統(tǒng)

4.2 運(yùn)動(dòng)控制仿真

為驗(yàn)證單棒轉(zhuǎn)移機(jī)械手運(yùn)動(dòng)控制系統(tǒng)的有效性，對機(jī)械手的各關(guān)節(jié)進(jìn)行了關(guān)節(jié)位置跟蹤仿真，跟蹤信號(hào)為正弦信號(hào)，其升降、旋轉(zhuǎn)、伸縮關(guān)節(jié)的跟蹤效果如圖11、圖12、圖13所示。由運(yùn)動(dòng)控制仿真結(jié)果可知，位置跟蹤精度較好，具有輕微的滯后，滯后時(shí)間約為0.05 s。

圖11 升降關(guān)節(jié)正弦跟蹤

圖12 旋轉(zhuǎn)關(guān)節(jié)正弦跟蹤

圖13 伸縮關(guān)節(jié)正弦跟蹤

4.3 繞流作用力仿真

單棒在水下運(yùn)動(dòng)速度的不同，水體對單棒的作用力也不同，為選取合適的單棒運(yùn)動(dòng)速度，對不同速度下的單棒繞流阻力、繞流升力和流致振動(dòng)頻率進(jìn)行了仿真研究，取單棒的運(yùn)動(dòng)速度范圍為0～5 m/s，仿真結(jié)果如圖14、圖15、圖16所示?？梢钥吹皆诘退贂r(shí)繞流阻力和繞流升力很小，隨著速度增大繞流阻力和繞流升力呈平方關(guān)系迅速增大，振動(dòng)頻率呈比例關(guān)系逐漸增大。

圖14 繞流阻力-速度

圖15 繞流升力-速度圖

圖16 流致振動(dòng)頻率-速度圖

4.4 綜合仿真

根據(jù)規(guī)劃的機(jī)械手末端軌跡，采用帶前饋補(bǔ)償?shù)腜D控制器對單棒轉(zhuǎn)移機(jī)械手進(jìn)行控制，計(jì)算單棒運(yùn)動(dòng)過程中繞流作用力，并將作用力施加在單棒上，進(jìn)行單棒轉(zhuǎn)移機(jī)械手運(yùn)動(dòng)的綜合仿真，機(jī)械手各關(guān)節(jié)軌跡跟蹤效果如圖17、圖18、圖19所示。單棒運(yùn)動(dòng)過程中繞流阻力、升力和流致振動(dòng)頻率如圖20、圖21、圖22所示。

圖17 升降關(guān)節(jié)規(guī)劃軌跡跟蹤圖

圖18 旋轉(zhuǎn)關(guān)節(jié)規(guī)劃軌跡跟蹤圖

圖19 伸縮關(guān)節(jié)規(guī)劃軌跡跟蹤圖

圖20 單棒擾流阻力圖

圖21 單棒擾流升力圖

圖22 單棒流致振動(dòng)頻率圖

根據(jù)仿真結(jié)果，帶前饋補(bǔ)償?shù)腜D控制器對機(jī)械手的控制具有較好的效果，跟蹤誤差和時(shí)間滯后都很小，驗(yàn)證了帶補(bǔ)償?shù)腜D控制器的控制效果。機(jī)械手在按照規(guī)劃的軌跡運(yùn)動(dòng)時(shí)，其受到的繞流阻力最大為0.3 N，最大繞流升力為1.7×10-2N，最大振動(dòng)頻率為2.8 Hz。表明規(guī)劃的軌跡沒有引起明顯繞流作用力，對單棒的轉(zhuǎn)運(yùn)具有較好的效果。

5 結(jié)論

本文針對相關(guān)組件水下立式縮容工藝中單棒水下轉(zhuǎn)移問題，提出一種采用貝塞爾曲線進(jìn)行軌跡規(guī)劃的方法，并設(shè)計(jì)了帶繞流阻力前饋的PD控制算法，用于機(jī)械手末端的運(yùn)動(dòng)控制。通過機(jī)械手運(yùn)動(dòng)仿真驗(yàn)證了規(guī)劃的軌跡方法在位置、速度、加速度上均連續(xù)，單棒運(yùn)轉(zhuǎn)平穩(wěn)，同時(shí)也驗(yàn)證了控制算法可有效補(bǔ)償繞流作用力，具有較好的穩(wěn)定性和可靠性。

在后續(xù)的研究工作中，可進(jìn)一步優(yōu)化單棒的運(yùn)動(dòng)速度，在轉(zhuǎn)運(yùn)效率和穩(wěn)定性的約束條件下研究機(jī)械手的最佳轉(zhuǎn)運(yùn)速度，同時(shí)可進(jìn)一步研究單棒運(yùn)轉(zhuǎn)中的流致振動(dòng)問題，設(shè)計(jì)穩(wěn)定性、魯棒性更佳的控制系統(tǒng)，并通過試驗(yàn)研究進(jìn)行驗(yàn)證和優(yōu)化。