本發明涉及空間微重力模擬領域，具體涉及一種中性浮力實驗中漂浮基座與機械臂的動態標度規劃方法，通過改變機械臂的慣性力和環境力，使基座位姿保持穩定。

背景技術：

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

技術實現要素：

本發明的目的在于針對上述現有技術中的問題，提供一種中性浮力實驗中漂浮基座與機械臂的動態標度規劃方法，對機械臂關節生成新的軌跡，調節機械臂關節角速度和角加速度，改變機械臂的慣性力和環境力，從而減小機械臂的運動對基座的動力學影響，使基座穩定。

為了實現上述目的，本發明采用的技術方案包括以下步驟：

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

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

第三步、求解標定因子；

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

3.2)將基座和機械臂的期望運動參數代入遞歸算法進行動力學逆解，確定軌跡；機械臂對基座產生的合力以及合力矩表示為τi(t)；基座在水下期望位置、期望姿態以及期望運動狀態下受到的合力及合力矩表示為τ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；

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

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

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

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

πi(ti)＝qei；

對應速度根據下列規則進行計算：

其中，給出時間區間[tk-1,tk]內分段的斜率；

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

得到拋物線的速度曲線：

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

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

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

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

r(t)＝ct；

其中c是一個大于1的常數，且

對公式進行兩次求導，得到關系式：

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

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

把其中后兩項線性化得到關系式：

公式變為：

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

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

附圖說明

圖1本發明規劃方法的整體流程圖；

圖2未進行動態標度的仿真實驗結果數據統計圖：

(a)基座位置數據圖；(b)基座姿態數據圖；

圖3未進行動態標度的仿真實驗中推進器推力數據圖：

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

圖4使用動態標度的仿真實驗結果數據統計圖：

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

圖5使用動態標度的仿真實驗結果數據統計圖：

(a)關節角數據統計圖；(b)關節角速度數據統計圖；

圖6使用動態標度的仿真實驗結果數據統計圖：

(a)基座位置數據圖；(b)基座姿態數據圖；

圖7使用動態標度的仿真實驗結果數據統計圖：

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

具體實施方式

下面結合附圖對本發明做進一步的詳細說明。

參見圖1，本發明動態標度規劃方法包括以下步驟：

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

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

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

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

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

πi(ti)＝qei；

對應速度根據下列規則進行計算：

其中，給出時間區間[tk-1,tk]內分段的斜率；

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

得到拋物線的速度曲線：

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

第三步、求解標定因子；

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

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

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

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

r(t)＝ct；

其中c是一個大于1的常數，且

對公式進行兩次求導，得到關系式：

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

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

把其中后兩項線性化得到關系式：

公式變為：

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

實施例

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

任務協同仿真過程可分為：

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

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

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

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

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

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

使用動態標度后，從圖中能夠看出基座三個自由度的位置誤差始終小于3×10^-3m，且基座三個轉動自由度的姿態誤差始終小于0.38°，而基座的期望姿態角為[15.79°0°-45°]。從圖中能夠看出推力輸出量最大的3號螺旋槳推進器的推進曲線比較好的限制在了10n內，使得螺旋槳能夠對基座位姿的穩定進行有效的控制。