[發(fā)明專(zhuān)利]一種空間機(jī)器人燃料最省的協(xié)調(diào)控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201811621147.7
• 申請(qǐng)日: 2018-12-28
• 公開(kāi)（公告）號(hào): CN109460069B
• 公開(kāi)（公告）日: 2021-03-23
• 發(fā)明（設(shè)計(jì)）人: 羅建軍;宗立軍;王明明;袁建平
• 申請(qǐng)（專(zhuān)利權(quán)）人: 西北工業(yè)大學(xué)
• 主分類(lèi)號(hào): G05D1/12
• 分類(lèi)號(hào): G05D1/12;G05B13/04
• 代理公司: 西北工業(yè)大學(xué)專(zhuān)利中心 61204
• 代理人: 劉新瓊

1.一種空間機(jī)器人燃料最省的協(xié)調(diào)控制方法，其特征在于步驟如下：

步驟1：基于飽和函數(shù)求解燃料最省最優(yōu)控制問(wèn)題

空間機(jī)器人系統(tǒng)質(zhì)心的動(dòng)力學(xué)模型為：

其中，表示施加在系統(tǒng)質(zhì)心上的作用力，代表系統(tǒng)質(zhì)心的線加速度，M代表系統(tǒng)的質(zhì)量；

定義為系統(tǒng)質(zhì)心的運(yùn)動(dòng)狀態(tài)，r_g,分別為系統(tǒng)質(zhì)心的位置和線速度，則系統(tǒng)質(zhì)心的狀態(tài)方程可以表示為：

考慮最小化燃料消耗以及實(shí)際情形下推進(jìn)器推力有限，空間機(jī)器人燃料最省的最優(yōu)控制器可以表示為：

其中，t_f表示末端時(shí)間，f_s,max為允許的最大輸入，x_s,0和分別為系統(tǒng)質(zhì)心的初始狀態(tài)和其期望的末端狀態(tài)；

使用間接法，式(3)表示的最優(yōu)控制問(wèn)題只能使用龐特里亞金極小值原理求解，本發(fā)明使用如下飽和函數(shù)表示輸入不等式約束：

其中，w＝[w₁,w₂,w₃]^T稱(chēng)為輔助輸入向量，參數(shù)k用于調(diào)節(jié)w_i＝0處的斜率

使用式(4)所示的飽和函數(shù)代替輸入不等式約束，最優(yōu)控制問(wèn)題(3)可以轉(zhuǎn)化為如下形式：

使用變分法求解式(5)表示的最優(yōu)控制問(wèn)題，Hamiltonian函數(shù)可以表示為：

其中，λ_r,λ_v分別為與位置和速度相關(guān)的協(xié)狀態(tài)，μ＝[μ₁,μ₂,μ₃]^T為拉格朗日乘子；

進(jìn)一步地，解的最優(yōu)性條件可以表示為：

因而，系統(tǒng)質(zhì)心處最優(yōu)控制輸入滿(mǎn)足微分方程約束(2)、(9)、(10)，代數(shù)方程約束(4)、(7)、(8)，以及邊值約束；使用配點(diǎn)法求解上述微分-代數(shù)方程系統(tǒng)的兩點(diǎn)邊值問(wèn)題可以得到最優(yōu)控制輸入因?yàn)橄到y(tǒng)質(zhì)心處的力只能由基座航天器推進(jìn)器的推力產(chǎn)生，所以同時(shí)得到了基座航天器推進(jìn)器的控制力

步驟2：基座姿態(tài)執(zhí)行器未飽和時(shí)設(shè)計(jì)協(xié)調(diào)控制律

空間機(jī)器人系統(tǒng)末端執(zhí)行器處不受外力、外力矩作用時(shí)系統(tǒng)的動(dòng)力學(xué)模型如式(11)所示：

其中，包含基座質(zhì)心的線加速度和基座角加速度，為關(guān)節(jié)角加速度，為基座慣量矩陣，為臂的慣量矩陣，為基座和臂的慣量耦合矩陣，分別為基座和臂與速度相關(guān)的非線性項(xiàng)，為基座質(zhì)心處所受的外力和外力矩，為臂關(guān)節(jié)輸入力矩，n為臂的關(guān)節(jié)數(shù)目；

從式(11)中消除基座質(zhì)心的線加速度變量，空間機(jī)器人反映臂運(yùn)動(dòng)對(duì)基座姿態(tài)影響的系統(tǒng)動(dòng)力學(xué)方程如式(12)所示：

其中，基座航天器推進(jìn)器的控制力f_bc由步驟1得到，為基座角加速度，為基座施加的外力矩，

下標(biāo)l₁:l₂,h₁:h₂代表原矩陣l₁到l₂行、h₁到h₂列形成的子矩陣；

在基座姿態(tài)執(zhí)行器未飽和時(shí)，設(shè)計(jì)基座姿態(tài)的參考角加速度和關(guān)節(jié)的參考角加速度分別如下：

其中，上標(biāo)‘d’代表期望值，單位四元數(shù)表示基座姿態(tài)，η_b,ξ_b分別為單位四元數(shù)的標(biāo)量和向量部分；代表基座姿態(tài)誤差，K_1,b,K_2,b,K_1,θ,K_2,θ為正定的增益矩陣；

將式(13)、(14)代入式(12)，可以得到基座姿態(tài)執(zhí)行器未飽和時(shí)基座姿態(tài)控制力矩和關(guān)節(jié)控制力矩如式(15)所示：

步驟3：基座姿態(tài)執(zhí)行器飽和時(shí)設(shè)計(jì)協(xié)調(diào)控制律

當(dāng)基座姿態(tài)執(zhí)行器飽和時(shí)，即式(15)中計(jì)算得到的基座姿態(tài)控制力矩超出了反作用飛輪基座姿態(tài)執(zhí)行器能夠提供的最大力矩|n_bc,i|n_{b,i max}，本發(fā)明使基座姿態(tài)執(zhí)行器提供其所能輸出的最大控制力矩，|n_bc,i|＝n_{b,i max}，并利用臂運(yùn)動(dòng)產(chǎn)生反作用力矩進(jìn)行基座姿態(tài)控制；

將式(12)的上半部分寫(xiě)為：

此時(shí)，可以設(shè)計(jì)關(guān)節(jié)的參考角加速度為：

其中，符號(hào)‘+’表示矩陣的廣義逆，

式(14)和式(17)分別表示關(guān)節(jié)運(yùn)動(dòng)對(duì)應(yīng)的兩個(gè)任務(wù)：將臂展開(kāi)至期望構(gòu)型以及產(chǎn)生適當(dāng)?shù)姆醋饔昧乜刂苹藨B(tài)；完整的關(guān)節(jié)參考角加速度通過(guò)任務(wù)優(yōu)先級(jí)方法得到：

其中，為單位矩陣，為矩陣的零空間；

因而，當(dāng)基座姿態(tài)執(zhí)行器飽和時(shí)，基座姿態(tài)控制力矩和關(guān)節(jié)控制力矩可以通過(guò)式(20)得到：