中性浮力實驗中漂浮基座與機械臂的動態(tài)標度規(guī)劃方法與流程

本發(fā)明涉及空間微重力模擬領(lǐng)域，具體涉及一種中性浮力實驗中漂浮基座與機械臂的動態(tài)標度規(guī)劃方法，通過改變機械臂的慣性力和環(huán)境力，使基座位姿保持穩(wěn)定。

背景技術(shù)：

中性浮力實驗是航天領(lǐng)域新興起的一種研究方法。所謂中性浮力是指當物體處于液體中時，如果物體的密度與液體的密度相同，則該物體能夠在液體中的任意一點懸浮。中性浮力模擬空間微重力效應(yīng)的方法也稱為液體浮力平衡重力方法，就是利用液體對物體的浮力抵消物體的重力，使物體處于懸浮狀態(tài)。已經(jīng)公開的技術(shù)有：yuichirotaira,shinichisagara,masahirooya.arobustcontrollerwithintegralactionforunderwatervehicle-manipulatorsystemsincludingthrusterdynamics[c].kumamoto,japan:internationalconferenceonadvancedmechatronicsystems,2014:415-420在已知uvms和推力器的動力學參數(shù)，而對水下有界擾動不確定的前提下，設(shè)計一種魯棒控制器，成功克服推進器的非線性對控制的不利影響，并且針對魯棒控制器的缺點在控制器中增加了積分環(huán)節(jié)。norimitsusakagami,mizuhoshibata,sadaokawamura.anattitudecontrolsystemforunderwatervehicle-manipulatorsystems[c].alaska,usa:ieeeinternationalconferenceonroboticsandautomation,2010:1761-1767.控制浮心和重心的相對位置來控制uvms的姿態(tài)，改變浮心通過移動浮塊來實現(xiàn)，文中設(shè)計的這套控制系統(tǒng)用于控制uvms的俯仰角，俯仰角的變化范圍從120°至150°，還能夠在執(zhí)行任務(wù)時保持整體系統(tǒng)的姿態(tài)。b.siciliano,l.sciavicco,l.villani,g.oriolo.robotics:modelling,planningandcontrol[m].newyork:springer,2009.介紹了針對固定基座機械臂的動態(tài)標度方法，解決機械臂關(guān)節(jié)力矩不足以實現(xiàn)期望軌跡的問題。但是在自由漂浮系統(tǒng)下，上述研究方法均未考慮基座的推力及其推力矩不足的情況。根據(jù)任務(wù)的需要，機械臂的軌跡一般是需要保持在某一位姿上的。但由于機械臂運動時會對基座產(chǎn)生干擾，如果基座維持位姿所需的力和力矩大于螺旋槳推進器所能提供的最大推力和推力矩，那么基座在水池坐標系中的實際位置和姿態(tài)指向與期望位置和姿態(tài)指向之間將產(chǎn)生較大的誤差，從而影響機械臂末端執(zhí)行器在水池坐標系中的軌跡。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于針對上述現(xiàn)有技術(shù)中的問題，提供一種中性浮力實驗中漂浮基座與機械臂的動態(tài)標度規(guī)劃方法，對機械臂關(guān)節(jié)生成新的軌跡，調(diào)節(jié)機械臂關(guān)節(jié)角速度和角加速度，改變機械臂的慣性力和環(huán)境力，從而減小機械臂的運動對基座的動力學影響，使基座穩(wěn)定。

為了實現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案包括以下步驟：

第一步、在水池坐標系中設(shè)定機械臂末端執(zhí)行器的軌跡，并在軌跡內(nèi)插入一個路徑點序列，進而將軌跡劃分為n段執(zhí)行；

第二步、在n段軌跡上建立n個三階多項式方程，將每段軌跡通過逆運動學方程計算成相應(yīng)的關(guān)節(jié)變量q(t)；

第三步、求解標定因子；

3.1)求解機械臂的期望軌跡，根據(jù)任務(wù)要求確定基座的期望位置、期望姿態(tài)以及期望運動狀態(tài)；

3.2)將基座和機械臂的期望運動參數(shù)代入遞歸算法進行動力學逆解，確定軌跡；機械臂對基座產(chǎn)生的合力以及合力矩表示為τi(t)；基座在水下期望位置、期望姿態(tài)以及期望運動狀態(tài)下受到的合力及合力矩表示為τw(t)；

3.3)把τs(t)和τw(t)通過公式分配給各個推進劑，分別表示為τspk(t)和τwpk(t)，其中k＝1,2,.....,6，求解分配到推進器k的|τspk(t)|在時間ti-1和ti之間的最大值|τspkmax(tkm)|，τwpk(t)在此時刻的值為τwpk(tkm)；

3.4)已知每個理想推進器的最大推力均為τpmax，每個推進器的標度因子ci(k)為：

3.5)選擇六個標度因子的最大值max(ci(k))作為機械臂期望軌跡的標度因子ci；

第四步、對重新標定的機械臂軌跡進行求解，在每段關(guān)節(jié)空間軌跡中通過相同的規(guī)則分別求得軌跡動態(tài)標度，形成多個新的軌跡，路徑相同而時間規(guī)律不同的運動軌跡表達式如下：

第一步軌跡路徑的參數(shù)表達式為：p＝f(s)；其中，s為路徑長度，p為末端執(zhí)行器在水池坐標系中的坐標；當t＝0時s＝0，當t＝tf時s＝sf。

第二步n個三階多項式方程定義為πi(t)，i＝1,2,…,n，通用約束條件為：

πi(ti-1)＝qei-1；

πi(ti)＝qei；

對應(yīng)速度根據(jù)下列規(guī)則進行計算：

其中，給出時間區(qū)間[tk-1,tk]內(nèi)分段的斜率；

選擇三次多項式：q(t)＝a3t³+a2t²+a1t+a0

得到拋物線的速度曲線：

通過求解如下方程，得到關(guān)節(jié)變量q(t)：

步驟3.2)中機械臂對基座產(chǎn)生的合力包括慣性力、流體力、浮力和重力；基座在水下期望位置、期望姿態(tài)以及期望運動狀態(tài)下受到的合力包括線纜力、流體力、重力和浮力。

第四步對重新標定的機械臂軌跡進行求解包括以下步驟：

其中，r(t)是使用軌跡動態(tài)標度形成的時間規(guī)律函數(shù)，q(t)是機械臂規(guī)劃求出的機械臂關(guān)節(jié)運動軌跡；對于定標函數(shù)，選擇簡單的線性函數(shù)形式：

r(t)＝ct；

其中c是一個大于1的常數(shù)，且

對公式進行兩次求導，得到關(guān)系式：

機械臂動力學方程中由速度和加速度產(chǎn)生的項可以表示成：

將相關(guān)公式代入，能夠得到函數(shù)r(t)重新標定過的機械臂動力學方程為：

把其中后兩項線性化得到關(guān)系式：

公式變?yōu)椋?imgfile="bda0001268454590000048.gif"wi="430"he="87"img-content="drawing"img-format="gif"orientation="portrait"inline="no"/>

機械臂動力學方程中與速度和加速度有關(guān)的項以c²的比例被減小或增大。

與現(xiàn)有技術(shù)相比，本發(fā)明具有如下的有益效果：某些任務(wù)要求下，機械臂的軌跡是建立在基座始終保持在某一位姿這一假設(shè)上的?；奈蛔朔€(wěn)定是依靠基座本身的靜穩(wěn)定性和螺旋槳推進器輸出的推力來維持，而如果基座穩(wěn)定所需的力和力矩大于螺旋槳推進器所能提供的最大推力和最大推力矩，那么基座就無法保持位姿的穩(wěn)定。本發(fā)明通過對運動軌跡的動態(tài)時間標定，形成新的機械臂關(guān)節(jié)角速度和角加速度，從而改變機械臂的慣性力和環(huán)境力，使得原先無法保持基座位姿穩(wěn)定的規(guī)劃方法實現(xiàn)基座位姿的穩(wěn)定。通過仿真實驗證實，本發(fā)明能夠使基座三個自由度的位置誤差及姿態(tài)誤差始終滿足要求，實用性以及可實施性較強。

附圖說明

圖1本發(fā)明規(guī)劃方法的整體流程圖；

圖2未進行動態(tài)標度的仿真實驗結(jié)果數(shù)據(jù)統(tǒng)計圖：

(a)基座位置數(shù)據(jù)圖；(b)基座姿態(tài)數(shù)據(jù)圖；

圖3未進行動態(tài)標度的仿真實驗中推進器推力數(shù)據(jù)圖：

(a)1-3號推進器推力數(shù)據(jù)圖；(b)4-6號推進器推力數(shù)據(jù)圖；

圖4使用動態(tài)標度的仿真實驗結(jié)果數(shù)據(jù)統(tǒng)計圖：

(a)oxy平面軌跡示意圖；(b)oxz平面軌跡示意圖；

圖5使用動態(tài)標度的仿真實驗結(jié)果數(shù)據(jù)統(tǒng)計圖：

(a)關(guān)節(jié)角數(shù)據(jù)統(tǒng)計圖；(b)關(guān)節(jié)角速度數(shù)據(jù)統(tǒng)計圖；

圖6使用動態(tài)標度的仿真實驗結(jié)果數(shù)據(jù)統(tǒng)計圖：

(a)基座位置數(shù)據(jù)圖；(b)基座姿態(tài)數(shù)據(jù)圖；

圖7使用動態(tài)標度的仿真實驗結(jié)果數(shù)據(jù)統(tǒng)計圖：

(a)1-3號推進器推力數(shù)據(jù)圖；(b)4-6號推進器推力數(shù)據(jù)圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明做進一步的詳細說明。

參見圖1，本發(fā)明動態(tài)標度規(guī)劃方法包括以下步驟：

第一步、在水池坐標系中設(shè)定機械臂末端執(zhí)行器的軌跡，并在軌跡內(nèi)插入一個路徑點序列，進而將軌跡劃分為n段執(zhí)行；

路徑的參數(shù)表達式為：p＝f(s)；其中，s為路徑長度，p為末端執(zhí)行器在水池坐標系中的坐標；當t＝0時s＝0，當t＝tf時s＝sf。

第二步、在n段軌跡上建立n個三階多項式方程，將每段軌跡通過逆運動學方程計算成相應(yīng)的關(guān)節(jié)變量q(t)；

n個三階多項式方程定義為πi(t)，i＝1,2,…,n，通用約束條件為：

πi(ti-1)＝qei-1；

πi(ti)＝qei；

對應(yīng)速度根據(jù)下列規(guī)則進行計算：

其中，給出時間區(qū)間[tk-1,tk]內(nèi)分段的斜率；

選擇三次多項式：q(t)＝a3t³+a2t²+a1t+a0

得到拋物線的速度曲線：

通過求解如下方程，得到關(guān)節(jié)變量q(t)：

第三步、求解標定因子；

3.1)求解機械臂的期望軌跡，根據(jù)任務(wù)要求確定基座的期望位置、期望姿態(tài)以及期望運動狀態(tài)；

3.2)將基座和機械臂的期望運動參數(shù)代入遞歸算法進行動力學逆解，確定軌跡；機械臂對基座產(chǎn)生的慣性力、流體力、浮力和重力的合力以及合力矩表示為τi(t)；基座在水下期望位置、期望姿態(tài)以及期望運動狀態(tài)下受到的線纜力、流體力、重力和浮力的合力及合力矩表示為τw(t)；

3.3)把τs(t)和τw(t)通過公式分配給各個推進劑，分別表示為τspk(t)和τwpk(t)，其中k＝1,2,.....,6，求解分配到推進器k的|τspk(t)|在時間ti-1和ti之間的最大值|τspkmax(tkm)|，τwpk(t)在此時刻的值為τwpk(tkm)；

3.4)已知每個理想推進器的最大推力均為τpmax，每個推進器的標度因子ci(k)為：

3.5)選擇六個標度因子的最大值max(ci(k))作為機械臂期望軌跡的標度因子ci；

第四步、對重新標定的機械臂軌跡進行求解，在每段關(guān)節(jié)空間軌跡中通過相同的規(guī)則分別求得軌跡動態(tài)標度，形成多個新的軌跡：

其中，r(t)是使用軌跡動態(tài)標度形成的時間規(guī)律函數(shù)，q(t)是機械臂規(guī)劃求出的機械臂關(guān)節(jié)運動軌跡；對于定標函數(shù)，選擇簡單的線性函數(shù)形式：

r(t)＝ct；

其中c是一個大于1的常數(shù)，且

對公式進行兩次求導，得到關(guān)系式：

機械臂動力學方程中由速度和加速度產(chǎn)生的項可以表示成：

將相關(guān)公式代入，能夠得到函數(shù)r(t)重新標定過的機械臂動力學方程為：

把其中后兩項線性化得到關(guān)系式：

公式變?yōu)椋?imgfile="bda0001268454590000086.gif"wi="438"he="86"img-content="drawing"img-format="gif"orientation="portrait"inline="no"/>

上式給出了路徑相同而時間規(guī)律不同的運動軌跡表達式。機械臂動力學方程中與速度和加速度有關(guān)的項以c²的比例被減小或增大。

實施例

在這組仿真實驗中設(shè)置基座的初始位置坐標為[000]，基座的初始姿態(tài)為[0°0°0°]，機械臂末端執(zhí)行器的初始關(guān)節(jié)角為[-30°30°]。任務(wù)目標是使機械臂末端執(zhí)行器到達坐標[0.50.50.2]，同時基座位置保持在[000]。仿真中的位置統(tǒng)一用水池坐標系中的坐標表示，單位是米。姿態(tài)統(tǒng)一用歐拉角表示，單位是度。在仿真試驗中基座使用基于趨近率的位置跟蹤滑膜控制器，參數(shù)c設(shè)為diag(2.1,2.1,2.1,8,8,8)，參數(shù)b設(shè)為diag(5.8,5.8,5.8,20,20,20)。機械臂使用pid控制器給關(guān)節(jié)角輸出控制信號，控制器增益系數(shù)kp設(shè)為diag(300,200)，微分增益系數(shù)kd設(shè)置為diag(70,50)，積分增益系數(shù)ki設(shè)為diag(0.1,0.1)。

任務(wù)協(xié)同仿真過程可分為：

(1)基座從0秒開始運動，根據(jù)前面規(guī)劃的運動軌跡在運動到期望姿態(tài)，軌跡的期望運動時間為10s。

(2)根據(jù)末端執(zhí)行器和目標點在水池坐標系中的坐標得到機械臂末端執(zhí)行器的操作空間軌跡。在操作空間軌跡中內(nèi)插5個點，將其分為6段，機械臂將在關(guān)節(jié)空間中執(zhí)行這六段軌跡，機械臂完整軌跡的期望運動時間為4s。

(3)使用動態(tài)標度方法對機械臂的軌跡進行重新定。標得到新的機械臂關(guān)節(jié)空間運動軌跡，完整軌跡的期望運動時間也隨之變化，機械臂沿著新的軌跡進行運動。

(4)在機械臂末端執(zhí)行器到達目標點后，基座和機械臂保持位置和姿態(tài)直到仿真實驗結(jié)束，仿真將在第25秒結(jié)束。

首先對基座與機械臂進行任務(wù)協(xié)同仿真，不執(zhí)行上述中的第(3)步，執(zhí)行任務(wù)協(xié)同仿真的第(1)(2)(3)步，即不對機械臂進行重新標定，仿真結(jié)果如圖2(a)、圖2(b)、圖3(a)以及圖3(b)所示，從圖中能夠看出多個推進器的推力輸出曲線都遠遠超出了極限推力10n。分析圖中的數(shù)據(jù)，x向位置誤差最大值有0.068m，y向位置誤差最大值有0.120m，z向位置誤差最大值有0.0027m，橫滾角誤差的最大值為3.08°，俯仰角θ誤差的最大值為1.15°，航向角ψ誤差的最大值為3.63°。從中還能夠發(fā)現(xiàn)，位姿誤差的最大值都出現(xiàn)在機械臂達到期望關(guān)節(jié)角時，這就說明在機械臂末端執(zhí)行器本該到達目標點時，而末端執(zhí)行器的位置誤差處于較大值。在這種情況下，由于所需的推力超出了螺旋槳的極限推力，會使得基座的位姿無法保持穩(wěn)定，導致任務(wù)失敗。

為了完成目標，需要保持基座的位姿穩(wěn)定。這里使用動態(tài)標度的方法來實現(xiàn)需要的軌跡規(guī)劃。執(zhí)行任務(wù)協(xié)同仿真的第(1)(2)(3)(4)步，仿真結(jié)果如圖4(a)，圖4(b)，圖5(a)，圖5(b)，圖6(a)，圖6(b)，圖7(a)，圖7(b)所示。

使用動態(tài)標度后，從圖中能夠看出基座三個自由度的位置誤差始終小于3×10^-3m，且基座三個轉(zhuǎn)動自由度的姿態(tài)誤差始終小于0.38°，而基座的期望姿態(tài)角為[15.79°0°-45°]。從圖中能夠看出推力輸出量最大的3號螺旋槳推進器的推進曲線比較好的限制在了10n內(nèi)，使得螺旋槳能夠?qū)蛔说姆€(wěn)定進行有效的控制。