本發(fā)明涉及航天領(lǐng)域的空間在軌服務(wù)技術(shù)，具體涉及一種適用于空間機(jī)器人執(zhí)行器故障下的快速姿態(tài)穩(wěn)定方法。

背景技術(shù)：

航天器在軌服務(wù)的對(duì)象多種多樣，其中動(dòng)力失效的衛(wèi)星或空間碎片占據(jù)很大比例。對(duì)于此類目標(biāo)，由于自身殘余角動(dòng)量的存在，往往處于旋轉(zhuǎn)狀態(tài)。當(dāng)空間機(jī)器人抓捕之后，包含機(jī)器人與目標(biāo)的整個(gè)系統(tǒng)也將處于旋轉(zhuǎn)狀態(tài)，此時(shí)若無(wú)法實(shí)現(xiàn)基座姿態(tài)的快速穩(wěn)定，將降低系統(tǒng)性能甚至可能導(dǎo)致機(jī)器人解體。

為滿足通信等任務(wù)的需求，空間機(jī)器人姿態(tài)具有指向性要求。在空間機(jī)器人執(zhí)行任務(wù)期間，存在著姿態(tài)控制系統(tǒng)出現(xiàn)故障的情形，此時(shí)若按照原有的姿態(tài)穩(wěn)定算法將可能導(dǎo)致基座失穩(wěn)甚至解體。因此研究空間機(jī)器人在執(zhí)行器出現(xiàn)故障下的基座姿態(tài)快速穩(wěn)定算法是十分必要的。

對(duì)于空間機(jī)器人而言，整個(gè)系統(tǒng)的轉(zhuǎn)動(dòng)慣量與系統(tǒng)構(gòu)型息息相關(guān)。若按照傳統(tǒng)考慮姿態(tài)單體系統(tǒng)的姿態(tài)穩(wěn)定算法，將有可能導(dǎo)致控制器性能的降低，甚至發(fā)生失穩(wěn)現(xiàn)象。在捕獲碎片之后，由于系統(tǒng)自身質(zhì)心的改變，將使得其相對(duì)于自身質(zhì)心的轉(zhuǎn)動(dòng)慣量非常大，而姿態(tài)控制系統(tǒng)所輸出的控制力矩幅值往往較小，此時(shí)即使控制率能夠?qū)崿F(xiàn)基座穩(wěn)定，但整個(gè)穩(wěn)定過(guò)程的調(diào)節(jié)時(shí)間非常長(zhǎng)。目前最常用的考慮執(zhí)行器故障下的姿態(tài)穩(wěn)定方法是考慮物體三軸姿態(tài)運(yùn)動(dòng)之間的耦合效果，通過(guò)控制兩軸的角運(yùn)動(dòng)進(jìn)行控制來(lái)實(shí)現(xiàn)整個(gè)系統(tǒng)的三軸穩(wěn)定。這種控制方法對(duì)于系統(tǒng)的轉(zhuǎn)動(dòng)慣量存在著約束，且整個(gè)過(guò)程調(diào)節(jié)時(shí)間長(zhǎng)，需要噴氣發(fā)動(dòng)機(jī)消耗大量燃料，這將縮短系統(tǒng)運(yùn)行周期，增加任務(wù)成本。同時(shí)現(xiàn)有研究成果只能解決姿態(tài)控制系統(tǒng)提供兩軸力矩的情況，對(duì)于只能提供單軸力矩的情形，仍然沒(méi)法實(shí)現(xiàn)基座的穩(wěn)定。

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

本發(fā)明的目的在于提供一種適用于空間機(jī)器人執(zhí)行器故障下的快速姿態(tài)穩(wěn)定方法，以克服上述現(xiàn)有技術(shù)存在的缺陷，本發(fā)明利用了空間機(jī)器人機(jī)械臂的冗余度與運(yùn)動(dòng)和基座運(yùn)動(dòng)之間的耦合效應(yīng)，能夠?qū)崿F(xiàn)在姿態(tài)控制系統(tǒng)僅能提供單軸力矩情況下的基座姿態(tài)快速穩(wěn)定，相比較于傳統(tǒng)針對(duì)單體系統(tǒng)的動(dòng)力學(xué)建模方法，能夠加快收斂速度，同時(shí)也能夠減少燃料的消耗。

為達(dá)到上述目的，本發(fā)明采用如下技術(shù)方案：

一種適用于空間機(jī)器人執(zhí)行器故障下的快速姿態(tài)穩(wěn)定方法，包括如下步驟：

1)利用遞推法建立捕獲目標(biāo)之后的空間機(jī)器人系統(tǒng)動(dòng)力學(xué)模型；

2)對(duì)所建立的動(dòng)力學(xué)模型進(jìn)行變量分離，得到逆動(dòng)力學(xué)模型；

3)通過(guò)對(duì)逆動(dòng)力學(xué)模型進(jìn)行處理得到機(jī)械臂運(yùn)動(dòng)與控制力矩之間的協(xié)調(diào)控制模型；

4)將設(shè)計(jì)的基座姿態(tài)運(yùn)動(dòng)角加速度與規(guī)劃的控制力矩信息輸入到協(xié)調(diào)控制模型中，最后根據(jù)基座始末狀態(tài)及平衡姿態(tài)，利用協(xié)調(diào)控制模型規(guī)劃出基座與機(jī)械臂運(yùn)動(dòng)軌跡。

進(jìn)一步地，步驟1)具體為：

將基座與慣性系的連接假設(shè)成一個(gè)六自由度虛鉸，其中三個(gè)表示轉(zhuǎn)動(dòng)，三個(gè)表示平動(dòng)，這樣包括基座在內(nèi)的整個(gè)空間機(jī)器人系統(tǒng)由若干個(gè)鉸連接組成，將各個(gè)鉸坐標(biāo)選取為系統(tǒng)廣義坐標(biāo)，利用遞推型多體系統(tǒng)動(dòng)力學(xué)建模方法，結(jié)合系統(tǒng)參數(shù)，建立捕獲旋轉(zhuǎn)目標(biāo)之后的系統(tǒng)動(dòng)力學(xué)模型，其表達(dá)式為：

其中，Z表示系統(tǒng)廣義質(zhì)量陣，z表示系統(tǒng)廣義力向量，表示系統(tǒng)廣義坐標(biāo)的二階導(dǎo)數(shù)，u表示各個(gè)鉸對(duì)應(yīng)的控制力元；

廣義質(zhì)量陣與廣義力向量具體表達(dá)式為：

Z＝α^T·mα+β^T·J·β

z＝α^T·(F⁰-mw)+β^T·(M⁰-J·σ-ε)+F^τq (2)

其中，α和β為系統(tǒng)基座與機(jī)械臂之間的鉸接方式及其上鉸點(diǎn)位置相關(guān)的運(yùn)算表達(dá)式，m和J皆為對(duì)角矩陣，其中m對(duì)角元為各個(gè)物體質(zhì)量，J對(duì)角元為各個(gè)物體在慣性系下的轉(zhuǎn)動(dòng)慣量，F⁰為基座與機(jī)械臂各關(guān)節(jié)所受外力組成的列向量，M⁰為基座與機(jī)械臂各關(guān)節(jié)所受外力矩組成的列向量，w表示由于基座與機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)而產(chǎn)生的慣性力，σ和ε表示由于基座與機(jī)械臂各關(guān)節(jié)平動(dòng)和轉(zhuǎn)動(dòng)所引起的慣性力矩，F^τq表示各個(gè)鉸之間的約束力元。

進(jìn)一步地，包括基座在內(nèi)的整個(gè)空間機(jī)器人系統(tǒng)由七個(gè)關(guān)節(jié)鉸及基座與慣性系之間的一個(gè)六自由度虛鉸組成。

進(jìn)一步地，步驟2)及步驟3)具體為：

對(duì)所建立的動(dòng)力學(xué)模型(1)，將控制力矩當(dāng)作未知量，鉸運(yùn)動(dòng)規(guī)律當(dāng)作已知量，進(jìn)行變量替換，得到逆動(dòng)力學(xué)模型，其具體表達(dá)式為：

其中，u_i表示i號(hào)鉸對(duì)應(yīng)的約束力元，z_i表示i號(hào)鉸對(duì)應(yīng)的廣義力分量，Z_i,j表示廣義質(zhì)量陣第i行第j列元素；

對(duì)于基座而言，其所對(duì)應(yīng)的鉸為基座與慣性系之間所連接的六自由度虛鉸，對(duì)應(yīng)的鉸關(guān)節(jié)控制力為基座所受的外力與外力矩，相應(yīng)的量在廣義力向量z中已考慮，因此u₁≡0，則此時(shí)具有式(4)：

對(duì)于式(4)，將已知量移至等式左邊，則其改寫(xiě)成式(5)的形式：

Hx＝b (5)

則方程(5)對(duì)應(yīng)的解為：

x＝H⁺b+(E-H⁺H)ξ (6)

式(6)即為機(jī)械臂運(yùn)動(dòng)與控制力矩之間的協(xié)調(diào)控制模型，其中，x表示需要確定的轉(zhuǎn)動(dòng)關(guān)節(jié)角加速度，H⁺表示矩陣H的廣義逆，E表示對(duì)應(yīng)維數(shù)的單位矩陣，ξ表示對(duì)應(yīng)維數(shù)的任意列向量。

進(jìn)一步地，步驟4)具體為：通過(guò)機(jī)械臂的運(yùn)動(dòng)實(shí)現(xiàn)對(duì)控制力矩的補(bǔ)償作用，使得空間機(jī)器人基座姿態(tài)實(shí)現(xiàn)快速穩(wěn)定，對(duì)于噴氣發(fā)動(dòng)機(jī)，其產(chǎn)生的控制力矩相對(duì)于基座本體系方位保持不變，方向?yàn)檎较蚧蚍捶较?，采用?7)進(jìn)行描述：

Mⁱⁿ＝A^bodyM^body (7)

其中，Mⁱⁿ表示在慣性系下的控制力矩，A^body表示基座方向余弦陣，M^body表示本體系下的控制力矩；

通過(guò)式(7)能夠得到平衡姿態(tài)時(shí)基座的方向余弦陣，繼而得到對(duì)應(yīng)的歐拉角，確定基座平衡姿態(tài)之后，對(duì)基座運(yùn)動(dòng)、機(jī)械臂運(yùn)動(dòng)與執(zhí)行器工作時(shí)間進(jìn)行規(guī)劃。

與現(xiàn)有技術(shù)相比，本發(fā)明具有以下有益的技術(shù)效果：

空間機(jī)器人機(jī)械臂運(yùn)動(dòng)與基座姿態(tài)運(yùn)動(dòng)之間存在著強(qiáng)耦合，傳統(tǒng)方法往往將這種耦合特性視作為一種干擾而進(jìn)行抑制，本發(fā)明在系統(tǒng)逆動(dòng)力學(xué)模型基礎(chǔ)上，通過(guò)利用這種耦合效果，能夠?qū)崿F(xiàn)在姿態(tài)控制系統(tǒng)僅能提供單軸力矩情況下的基座姿態(tài)快速穩(wěn)定，相比較于傳統(tǒng)針對(duì)單體系統(tǒng)的動(dòng)力學(xué)建模方法，能夠加快收斂速度，同時(shí)也能夠減少燃料的消耗，實(shí)現(xiàn)機(jī)器人基座姿態(tài)在噴氣發(fā)動(dòng)機(jī)部分失效下的快速穩(wěn)定。

附圖說(shuō)明

圖1為協(xié)調(diào)控制仿真模型；其中，1-7均為關(guān)節(jié)鉸；

圖2為情況一，基座姿態(tài)歐拉角變化；

圖3為情況一，關(guān)節(jié)鉸1-4角速度變化；

圖4為情況一，關(guān)節(jié)鉸5-7角速度變化；

圖5為情況二，基座姿態(tài)歐拉角變化；

圖6為情況二，關(guān)節(jié)鉸1-4角速度變化；

圖7為情況二，關(guān)節(jié)鉸5-7角速度變化。

具體實(shí)施方式

下面對(duì)本發(fā)明作進(jìn)一步詳細(xì)描述：

本發(fā)明要解決的技術(shù)問(wèn)題是在在考慮執(zhí)行器故障下空間機(jī)器人捕獲旋轉(zhuǎn)目標(biāo)之后的基座姿態(tài)快速穩(wěn)定。其主要原理在于：首先利用遞推法建立捕獲目標(biāo)之后的空間機(jī)器人系統(tǒng)動(dòng)力學(xué)模型，然后得到系統(tǒng)逆動(dòng)力學(xué)模型，通過(guò)對(duì)模型進(jìn)行處理得到機(jī)械臂運(yùn)動(dòng)與控制力矩之間的協(xié)調(diào)控制模型，將設(shè)計(jì)的基座姿態(tài)運(yùn)動(dòng)角加速度與規(guī)劃的控制力矩信息輸入到控制模型中，考慮到噴氣發(fā)動(dòng)機(jī)所產(chǎn)生控制力矩相對(duì)基座本體系方向固定的工作特點(diǎn)，對(duì)應(yīng)控制力矩能夠消除系統(tǒng)角動(dòng)量的平衡姿態(tài)，最后根據(jù)基座始末狀態(tài)及平衡姿態(tài)，利用協(xié)調(diào)控制模型規(guī)劃出基座與機(jī)械臂運(yùn)動(dòng)軌跡。機(jī)械臂由電機(jī)驅(qū)動(dòng)，現(xiàn)有的電機(jī)控制回路具有很好的跟蹤效果，能夠使電機(jī)按照所設(shè)計(jì)的規(guī)律進(jìn)行運(yùn)動(dòng)。相比較于傳統(tǒng)空間機(jī)器人領(lǐng)域中的減小機(jī)械臂對(duì)基座干擾的控制策略，本方法利用了空間機(jī)器人機(jī)械臂的冗余度與運(yùn)動(dòng)和基座運(yùn)動(dòng)之間的耦合效應(yīng)，能夠?qū)崿F(xiàn)在姿態(tài)控制系統(tǒng)僅能提供單軸力矩情況下的基座姿態(tài)快速穩(wěn)定。相比較于傳統(tǒng)針對(duì)單體系統(tǒng)的動(dòng)力學(xué)建模方法，能夠加快收斂速度，同時(shí)也能夠減少燃料的消耗。

本發(fā)明的方法具體包括以下步驟：

步驟一：為簡(jiǎn)化動(dòng)力學(xué)建模，將基座與慣性系之前的連接假設(shè)成一個(gè)六自由度虛鉸(附圖說(shuō)明中關(guān)節(jié)鉸指的是機(jī)械臂關(guān)節(jié)鉸)(三個(gè)表示轉(zhuǎn)動(dòng)，三個(gè)表示平動(dòng))，這樣就可以將包括基座在內(nèi)的整個(gè)系統(tǒng)看做由多個(gè)鉸連接(不僅包括七個(gè)關(guān)節(jié)鉸還包括基座與慣性系之間假設(shè)的一個(gè)六自由度虛鉸)所組成。將各個(gè)鉸坐標(biāo)選取為廣義坐標(biāo)，利用遞推型多體系統(tǒng)動(dòng)力學(xué)建模方法，結(jié)合系統(tǒng)參數(shù)，建立捕獲旋轉(zhuǎn)目標(biāo)之后的系統(tǒng)動(dòng)力學(xué)模型，其表達(dá)式為：

其中，Z表示系統(tǒng)廣義質(zhì)量陣，z表示系統(tǒng)廣義力向量，表示系統(tǒng)廣義坐標(biāo)的二階導(dǎo)數(shù)，u表示各個(gè)鉸對(duì)應(yīng)的控制力元。廣義質(zhì)量陣與廣義力向量具體表達(dá)式為：

Z＝α^T·mα+β^T·J·β

z＝α^T·(F⁰-mw)+β^T·(M⁰-J·σ-ε)+F^τq (2)

其中，α和β為系統(tǒng)基座與機(jī)械臂之間的鉸接方式及其上鉸點(diǎn)位置相關(guān)的運(yùn)算表達(dá)式，m和J皆為對(duì)角矩陣，其中m對(duì)角元為各個(gè)物體質(zhì)量，J對(duì)角元為各個(gè)物體在慣性系下的轉(zhuǎn)動(dòng)慣量。F⁰為基座與機(jī)械臂各關(guān)節(jié)所受外力組成的列向量，M⁰為基座與機(jī)械臂各關(guān)節(jié)所受外力矩組成的列向量，w表示由于基座與機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)而產(chǎn)生的慣性力，σ和ε表示由于基座與機(jī)械臂各關(guān)節(jié)平動(dòng)和轉(zhuǎn)動(dòng)所引起的慣性力矩，F^τq表示鉸之間的約束力元。

步驟二：對(duì)所建立的動(dòng)力學(xué)模型(1)，將控制力矩當(dāng)作未知量，鉸運(yùn)動(dòng)規(guī)律當(dāng)作已知量，進(jìn)行變量替換，得到逆動(dòng)力學(xué)模型，其具體表達(dá)式為：

其中，u_i表示i號(hào)鉸對(duì)應(yīng)的約束力元，z_i表示i號(hào)鉸對(duì)應(yīng)的廣義力分量，Z_i,j表示廣義質(zhì)量陣第i行第j列元素。

對(duì)于基座而言，其所對(duì)應(yīng)的鉸為基座與慣性系之間所連接的六自由度虛鉸，對(duì)應(yīng)的鉸關(guān)節(jié)控制力為基座所受的外力與外力矩，相應(yīng)的量在廣義力向量z中已考慮，因此，可以看做u₁≡0(為方便建立系統(tǒng)動(dòng)力學(xué)模型，將基座與慣性坐標(biāo)系原點(diǎn)之間的連接也看作是一個(gè)鉸，這樣對(duì)于機(jī)械臂而言，其鉸關(guān)節(jié)所代表的鉸號(hào)將為機(jī)械臂關(guān)節(jié)號(hào)加一，即機(jī)械臂1號(hào)關(guān)節(jié)鉸對(duì)應(yīng)2號(hào)鉸，2號(hào)關(guān)節(jié)鉸對(duì)應(yīng)3號(hào)鉸，以此類推)。則此時(shí)具有式(4)：

對(duì)于式(4)，將已知量移至等式左邊，則其改寫(xiě)成式(5)的形式：

Hx＝b (5)

則方程(5)對(duì)應(yīng)的解為：

x＝H⁺b+(E-H⁺H)ξ (6)

式(6)即為機(jī)械臂運(yùn)動(dòng)與控制力矩之間的協(xié)調(diào)控制模型。其中，x表示需要確定的轉(zhuǎn)動(dòng)關(guān)節(jié)角加速度，H⁺表示矩陣H的廣義逆，E表示對(duì)應(yīng)維數(shù)的單位矩陣，ξ表示對(duì)應(yīng)維數(shù)的任意列向量。

步驟三：通過(guò)機(jī)械臂的運(yùn)動(dòng)實(shí)現(xiàn)對(duì)控制力矩的補(bǔ)償作用，使得空間機(jī)器人基座姿態(tài)實(shí)現(xiàn)快速穩(wěn)定。對(duì)于噴氣發(fā)動(dòng)機(jī)，其產(chǎn)生的控制力矩相對(duì)于基座本體系方位保持不變，方向?yàn)檎较蚧蚍捶较颉？捎檬?7)進(jìn)行描述。

Mⁱⁿ＝A^bodyM^body (7)

其中，Mⁱⁿ表示在慣性系下的控制力矩，A^body表示基座方向余弦陣，M^body表示本體系下的控制力矩。

由于噴氣發(fā)動(dòng)機(jī)的特點(diǎn)，其產(chǎn)生的幅值固定?？紤]到執(zhí)行器方向與幅值的約束和故障情況，需要確定控制力矩能夠平衡系統(tǒng)角動(dòng)量時(shí)基座所對(duì)應(yīng)的平衡姿態(tài)。當(dāng)控制力矩能夠平衡角動(dòng)量時(shí)，控制力矩方向與角動(dòng)量的方向在慣性控制保持平行。噴氣發(fā)動(dòng)機(jī)所產(chǎn)生的控制力矩僅用于平衡系統(tǒng)角動(dòng)量，由于噴氣發(fā)動(dòng)機(jī)存在部分失效，所產(chǎn)生的相應(yīng)控制力矩賦值和方向均受限，因此需要得到發(fā)動(dòng)機(jī)工作時(shí)對(duì)應(yīng)的姿態(tài)平衡姿態(tài)。通過(guò)式(7)能夠得到平衡姿態(tài)時(shí)基座的方向余弦陣，繼而得到對(duì)應(yīng)的歐拉角，確定基座平衡姿態(tài)，根據(jù)基座平衡姿態(tài)與理想姿態(tài)是否一致存在兩種情形。對(duì)于情況一：平衡姿態(tài)與理想姿態(tài)一致，此時(shí)在基座達(dá)到平衡姿態(tài)之后，噴氣發(fā)動(dòng)機(jī)開(kāi)始工作直至系統(tǒng)角動(dòng)量變?yōu)榱?，機(jī)械臂運(yùn)動(dòng)按照式(4)-(6)進(jìn)行規(guī)劃；對(duì)于情況二：平衡姿態(tài)與理想姿態(tài)不一致，此時(shí)基座需要先機(jī)動(dòng)到平衡姿態(tài)，噴氣發(fā)動(dòng)機(jī)開(kāi)始工作，當(dāng)整個(gè)系統(tǒng)角動(dòng)量為零之后，依靠機(jī)械臂運(yùn)動(dòng)使基座從平衡姿態(tài)機(jī)動(dòng)到理想姿態(tài)，機(jī)械臂運(yùn)動(dòng)仍然按照式(4)-(6)進(jìn)行規(guī)劃。

步驟四，確定基座平衡姿態(tài)之后，開(kāi)始對(duì)基座運(yùn)動(dòng)、機(jī)械臂運(yùn)動(dòng)與執(zhí)行器工作時(shí)間進(jìn)行規(guī)劃。對(duì)于基座運(yùn)動(dòng)而言，首先是在約束條件下實(shí)現(xiàn)從初始姿態(tài)到平衡姿態(tài)的最快姿態(tài)轉(zhuǎn)移，在此過(guò)程中僅僅依靠機(jī)械臂運(yùn)動(dòng)來(lái)實(shí)現(xiàn)。隨后發(fā)動(dòng)機(jī)開(kāi)始工作，基座在控制力矩與機(jī)械臂運(yùn)動(dòng)共同作用下保持在平衡姿態(tài)，當(dāng)系統(tǒng)角動(dòng)量為零時(shí)，發(fā)動(dòng)機(jī)將停止工作。若平衡姿態(tài)為基座所需的姿態(tài)，則在發(fā)動(dòng)機(jī)工作過(guò)程中，將設(shè)計(jì)部分鉸的運(yùn)動(dòng)以實(shí)現(xiàn)系統(tǒng)最終的穩(wěn)定，包括基座與機(jī)械臂；若平衡姿態(tài)不為基座所需的姿態(tài)，則在發(fā)動(dòng)機(jī)停止工作之后，需要基座在約束條件下實(shí)現(xiàn)姿態(tài)機(jī)動(dòng)，在能夠滿足基座運(yùn)動(dòng)的前提下選擇部分關(guān)節(jié)使其運(yùn)動(dòng)停止。

為了更好地說(shuō)明本發(fā)明的目的和優(yōu)點(diǎn)，下面結(jié)合附圖和實(shí)例對(duì)本發(fā)明內(nèi)容做進(jìn)一步說(shuō)明：

設(shè)空間機(jī)器人已經(jīng)捕獲目標(biāo)，機(jī)械臂末端關(guān)節(jié)與目標(biāo)之間不存在相對(duì)運(yùn)動(dòng)以保持穩(wěn)定，同時(shí)目標(biāo)慣性參數(shù)已知，仿真模型如圖1所示。整個(gè)機(jī)械臂包含有7個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)，將最靠近基座的關(guān)節(jié)編號(hào)為“1”，末端關(guān)節(jié)編號(hào)為“7”。為了充分考慮到執(zhí)行器故障的情況，對(duì)兩種典型情況進(jìn)行了仿真。在情況一中，考慮執(zhí)行器故障情況下的平衡姿態(tài)即為基座末狀態(tài)；而對(duì)于情況二，基座平衡姿態(tài)與末狀態(tài)不一致。假設(shè)基座歐拉角最大角加速度幅值受限，應(yīng)用本方法，可以實(shí)現(xiàn)在執(zhí)行器只能提供單軸力矩下的基座姿態(tài)快速穩(wěn)定，具體包括以下步驟：

步驟一：先根據(jù)遞推型多體系統(tǒng)建模方法建立整個(gè)系統(tǒng)動(dòng)力學(xué)模型。將系統(tǒng)初始狀態(tài)，包括系統(tǒng)構(gòu)型、基座運(yùn)動(dòng)狀態(tài)、機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)狀態(tài)代入模型，得到數(shù)值模型，按照式(3)得帶系統(tǒng)逆動(dòng)力學(xué)模型，并得到如式(4)對(duì)應(yīng)的控制模型；

步驟二：由式(7)計(jì)算得到基座的方向余弦陣，繼而得到對(duì)應(yīng)的的平衡姿態(tài)歐拉角，在仿真算例中，假設(shè)基座歐拉角最大角加速度為需要的目標(biāo)姿態(tài)歐拉角為：q＝[0 0 0]^T，基座初始角速度分別為ω₀＝[0.024 -0.014 -0.019]^T和ω₀＝[0.007 0.028 0.034]^T，按式(7)計(jì)算基座的平衡姿態(tài)所對(duì)應(yīng)的歐拉角分別為：q_b＝[0 0 0]^T和q＝[0.092 -0.403 0.441]^T。

步驟三：結(jié)合式(6)，在基座歐拉角最大角加速度受限的約束下，分別得到出基座姿態(tài)與機(jī)械臂關(guān)節(jié)角速度隨時(shí)間變化的規(guī)律，代入到數(shù)值積分模塊中，采用四階RK公式進(jìn)行計(jì)算。

本實(shí)例中采用的系統(tǒng)參數(shù)的值如表1所示。

表1模型參數(shù)

圖2-4對(duì)應(yīng)情況一，即基座平衡姿態(tài)與穩(wěn)定姿態(tài)一致，從圖2中可以看出，整個(gè)基座姿態(tài)運(yùn)動(dòng)與規(guī)劃一致，當(dāng)基座機(jī)動(dòng)到穩(wěn)定姿態(tài)之后，在外力矩與機(jī)械臂共同作用下保持姿態(tài)的穩(wěn)定；圖3與圖4展現(xiàn)了整個(gè)過(guò)程中機(jī)械臂的運(yùn)動(dòng)規(guī)律，最終機(jī)械臂系統(tǒng)能夠停止運(yùn)動(dòng)。圖5-7對(duì)應(yīng)情況二，即基座平衡姿態(tài)與穩(wěn)定姿態(tài)不一致，情況二與情況一之間主要區(qū)別體現(xiàn)在基座姿態(tài)運(yùn)動(dòng)規(guī)律上，從圖5可以看出，基座先要機(jī)動(dòng)到平衡姿態(tài)，待系統(tǒng)角動(dòng)量為零之后，再回到平衡姿態(tài)，與所規(guī)劃的基座運(yùn)動(dòng)規(guī)律一致。