一種空間柔性機(jī)械臂振動(dòng)抑制算法
【專利摘要】本發(fā)明公開(kāi)了一種空間柔性機(jī)械臂振動(dòng)抑制算法，包含以下步驟：S1、在廣義坐標(biāo)系下，建立剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型；S2、對(duì)剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型中的廣義變量進(jìn)行分解，得到快變參數(shù)和慢變參數(shù)；S3、分別針對(duì)快變參數(shù)和慢變參數(shù)建立相應(yīng)的子系統(tǒng)，并配置對(duì)應(yīng)的控制律；S4、將快變子系統(tǒng)的控制律和慢變子系統(tǒng)的控制律進(jìn)行復(fù)合，以定位剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的位置并進(jìn)行振動(dòng)抑制。本發(fā)明通過(guò)建立剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型，并將動(dòng)力學(xué)模型分解為不同尺度慢變和快變兩個(gè)子系統(tǒng)，將平臺(tái)的剛性姿態(tài)運(yùn)動(dòng)狀態(tài)與撓性振動(dòng)狀態(tài)分開(kāi)，分別設(shè)計(jì)控制器進(jìn)行組合控制，有效解決了在軌操控柔性機(jī)械臂撓性振動(dòng)控制問(wèn)題。
【專利說(shuō)明】
一種空間柔性機(jī)械臂振動(dòng)抑制算法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及在軌操控航天器姿態(tài)控制技術(shù)領(lǐng)域，尤其涉及一種空間柔性機(jī)械臂振 動(dòng)抑制算法。
【背景技術(shù)】
[0002] 我國(guó)的航天"三步走"（載人航天一空中對(duì)接一建立空間站)戰(zhàn)略正順利實(shí)施，現(xiàn)已 完成了航天員出艙活動(dòng)，下一步要實(shí)現(xiàn)航天器的對(duì)接和空間站的建設(shè)。像我國(guó)這樣航天經(jīng) 費(fèi)活動(dòng)并不十分充裕的發(fā)展中國(guó)家，利用空間柔性機(jī)械臂進(jìn)行太空操作更為合適，在節(jié)省 巨額開(kāi)支的同時(shí)又能避免不必要的人員傷亡，因此對(duì)空間柔性機(jī)械臂開(kāi)展多項(xiàng)關(guān)鍵技術(shù)的 研究具有重要的理論價(jià)值和工程應(yīng)用價(jià)值。
[0003] 空間柔性機(jī)械臂執(zhí)行操作任務(wù)時(shí)，不僅要求控制器對(duì)各關(guān)節(jié)的位置實(shí)現(xiàn)高精度軌 跡跟蹤，而且必須快速抑制由大柔度臂桿的柔性引發(fā)的殘余振動(dòng)，以減少振動(dòng)衰減的等待 時(shí)間，提高機(jī)械臂的工作效率。然而，對(duì)于具有柔性臂桿的空間機(jī)械臂系統(tǒng)，其振動(dòng)控制器 的設(shè)計(jì)面臨以下幾個(gè)難題:首先，由關(guān)節(jié)端輸入的驅(qū)動(dòng)力矩到臂桿末端的端點(diǎn)位置輸出間 的傳遞函數(shù)具有非最小相位特征，特別是在關(guān)節(jié)軸上有一個(gè)正的輸入而在末端有負(fù)的位移 時(shí)表現(xiàn)更加明顯;其次，機(jī)械臂操作負(fù)載和邊界條件都是時(shí)變的，振動(dòng)模態(tài)參數(shù)具有時(shí)變性 和不確定性;再次，建模時(shí)的模態(tài)截?cái)嗉夹g(shù)會(huì)引起控制溢出和觀測(cè)溢出，導(dǎo)致剩余模態(tài)的激 勵(lì)，使控制性能變壞甚至失穩(wěn)。這些因素使得柔性機(jī)械臂的末端精確定位控制不易實(shí)現(xiàn)，若 僅把關(guān)節(jié)位置作為系統(tǒng)的輸出量進(jìn)行控制，雖然可以將傳遞函數(shù)轉(zhuǎn)化為最小相位，但顯然 無(wú)法進(jìn)行振動(dòng)抑制，必將引起很大的動(dòng)態(tài)跟蹤誤差;若以系統(tǒng)的名義截?cái)嗄Ｐ瓦M(jìn)行控制器 設(shè)計(jì)，其魯棒性又必須得以保證。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的在于提供一種空間柔性機(jī)械臂振動(dòng)抑制算法，通過(guò)建立剛?cè)狁詈蠙C(jī) 械臂系統(tǒng)的動(dòng)力學(xué)模型，并將動(dòng)力學(xué)模型分解為不同尺度慢變和快變兩個(gè)子系統(tǒng)，將平臺(tái) 的剛性姿態(tài)運(yùn)動(dòng)狀態(tài)與撓性振動(dòng)狀態(tài)分開(kāi)，分別設(shè)計(jì)控制器進(jìn)行組合控制，有效解決了在 軌操控柔性機(jī)械臂撓性振動(dòng)控制問(wèn)題。
[0005] 為了達(dá)到上述目的，本發(fā)明通過(guò)以下技術(shù)方案實(shí)現(xiàn):一種空間柔性機(jī)械臂振動(dòng)抑 制算法，其特點(diǎn)是，用于剛?cè)狁詈蠙C(jī)械臂系統(tǒng)中，所述的剛?cè)狁詈蠙C(jī)械臂系統(tǒng)包含依次連接 的中心體、第一柔性機(jī)械臂、第二柔性機(jī)械臂及一剛性機(jī)械臂，該空間柔性機(jī)械臂振動(dòng)抑制 算法包含以下步驟：
[0006] S1、在廣義坐標(biāo)系下，建立剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型；
[0007] S2、對(duì)剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型中的廣義變量進(jìn)行分解，得到快變參數(shù) 和慢變參數(shù)；
[0008] S3、分別針對(duì)快變參數(shù)和慢變參數(shù)建立相應(yīng)的子系統(tǒng)，并配置對(duì)應(yīng)的控制律；
[0009] S4、將快變子系統(tǒng)的控制律和慢變子系統(tǒng)的控制律進(jìn)行復(fù)合，以定位剛?cè)狁詈蠙C(jī) 械臂系統(tǒng)的位置并進(jìn)行振動(dòng)抑制。
[0010] 所述的快變參數(shù)包含柔性機(jī)械臂微振動(dòng)的模態(tài)坐標(biāo)。
[0011] 所述的慢變參數(shù)包含中心體的平動(dòng)位移、中心體的轉(zhuǎn)動(dòng)姿態(tài)角以及機(jī)械臂各個(gè)關(guān) 節(jié)位移量。
[0012] 所述的步驟S1包含：
[0013] 在中心體質(zhì)心位置建立本體坐標(biāo)系St，選取廣義坐標(biāo)/=[擴(kuò)<的< <]， 其中，X為中心體平動(dòng)變量，物為中心體本體坐標(biāo)系相對(duì)慣性坐標(biāo)系的姿態(tài)角，φι為與中心 體相連的第一柔性機(jī)械臂關(guān)節(jié)旋轉(zhuǎn)角度，物為第一柔性機(jī)械臂與第二柔性臂之間旋轉(zhuǎn)關(guān) 節(jié)的轉(zhuǎn)角，Φ3為剛性機(jī)械臂相對(duì)于第二柔性機(jī)械臂的旋轉(zhuǎn)角度，^為第一柔性機(jī)械臂的模 態(tài)坐標(biāo)，Τ2為第二柔性機(jī)械臂的模態(tài)坐標(biāo)；
[0014] 設(shè)偏速度列陣為# =[< Pf]，可知 Wx =X;W. =pb=0b；fV3 =φ, =￡ll；fV4 =iv\Wi=V，\Wb =t7;W7 =ψ3 5

[0023]得到完整的動(dòng)力學(xué)方程，
[0024] 其中，中心體平動(dòng)方程形式表示為：
[0025] mX - AchS；xmb - - AclSf：U(p2 - Ae,S；n, + + Ae2P：t2 = Qx
[0026] 中心體轉(zhuǎn)動(dòng)方程形式表示為：
[0027] -S^A^X + Jib0b + IbA + ΙΒ2ηφ2 + Jb3ii, + H；^ + = Q%
[0028]第一柔性機(jī)械臂的轉(zhuǎn)動(dòng)方程形式表不為：
[0029] + />, + /Π Ω, + Ι12Πφ2 + /13il3 + Κτχ + Η；2τ2 = βΩ,
[0030] 第二柔性機(jī)械臂的轉(zhuǎn)動(dòng)方程形式表示為：
[0031] -nTSlxTA2cX+ +n rJ22n^ +UTjJl3+TiTH：lrl+n TH：2f2=Qlu
[0032] 剛性機(jī)械臂的轉(zhuǎn)動(dòng)方程形式表示為：
[0033] -S^Aie± + +.^0, + Ι^Πφ, + ^3Ω.3 + ￡Τ?#, + Η^?2 =?〇3
[0034]第一柔性機(jī)械臂的振動(dòng)方程形式表不為：
[0035] P；TAlcX + H；l mb + + JUp, + Η；^?3 +(ll+Wll)rl+ Wl2f2 + Α,τ, = %
[0036] 第二柔性機(jī)械臂的振動(dòng)方程形式表示為：
[0037] + Κ?+ β·2ΤΠ^ + //3Τ??3 + + (I2 + W2, )τ2 + A2t2 = gr,
[0038] 其中，Jbb為衛(wèi)星相對(duì)整星質(zhì)心的轉(zhuǎn)動(dòng)慣量矩陣，為._.=_^>7?&為衛(wèi)星中心體的姿態(tài)角 速度，內(nèi)鉸處的連體坐標(biāo)系已設(shè)定為<=(m)，以此類推;第一柔性機(jī)械臂、第二柔性 機(jī)械臂級(jí)一剛性機(jī)械臂之間只有一個(gè)旋轉(zhuǎn)關(guān)節(jié)，即有Α=<Ω 1λ Ω2=Π 色、Π = [0 0 表示沿著Z軸旋轉(zhuǎn)。
[0039] 所述的剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型的標(biāo)準(zhǔn)形式如下：
[0041]式中，Θ包含廣義坐標(biāo)中中心體平動(dòng)和轉(zhuǎn)動(dòng)六個(gè)變量、三個(gè)機(jī)械臂的旋轉(zhuǎn)關(guān)節(jié)角 度，q包含了二個(gè)柔性機(jī)械臂的振動(dòng)坐標(biāo)，F(xiàn)e為與Θ變量對(duì)應(yīng)的廣義外力，F(xiàn)q為與變量對(duì)應(yīng)的 廣義外力，G為各哥氏力，τ為廣義模態(tài)力，K為剛性矩陣；
[0042]剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的質(zhì)量矩陣M(0，q)是一正定矩陣，設(shè)它的逆陣為H(0，q)，那 么上式變?yōu)椋?br>[0044] 上式可分別寫(xiě)為：
[0047]定義最小剛度系數(shù)為k=min(kii)，定義μ=?Λ，引入新的變量z = kq，貝1J有q = yz， 定義#.= //·化，將新的變量代入上式得到：
[0050] 所述的步驟S2中包含：
[0052] [Mlls(0,0)]-1 = Hlls(0,〇)-Hl2s(0,〇)[H22s(0,0)]-1H21s(0,〇);
[0053] 且(7"(狄次/c/d)=((，(認(rèn)次/""：、"::) = 0
[0054] 式中下標(biāo)"s"表示向量處于慢變子系統(tǒng)，即變量在慢時(shí)標(biāo)中的計(jì)算；
[0057]引入快變時(shí)標(biāo)# = 〃 士，定義新的狀態(tài)變量zfl Z Zs，S,2 ~ ，式中下f 表,J、 變量處于快變子系統(tǒng)，于是式(1)可變換為：
[0061 ]由于慢變時(shí)標(biāo)與快變時(shí)標(biāo)相互獨(dú)立，且在邊界層的區(qū)域μ-O內(nèi)慢變分量可視為常 數(shù)(即灰/ # = = 0)，此時(shí)，如果令μ = 〇，得到：
[0063] 綜合狀態(tài)變量Zfl及上式，可得到以狀態(tài)方程形式描述的快變子系統(tǒng)為：
[0064] Zf=Afzf +BfTf
[0065]其中，
[0069] Tf為快變子系統(tǒng)的控制輸入，用于抑制柔性振動(dòng)，" ?"為對(duì)快變時(shí)標(biāo)ξ求導(dǎo)；剛?cè)?親合機(jī)械臂系統(tǒng)的總控制輸入為T = TS+Tf;原系統(tǒng)的狀態(tài)變量近似為θ = θ3+0(μ)，z = Zs+Zfl +〇(μ)，當(dāng)μ充分小時(shí)，可忽略高階無(wú)窮小項(xiàng)〇(μ)。
[0070] 所述的步驟S3中配置慢變子系統(tǒng)的控制律包含：
[0071] 設(shè)慢變子系統(tǒng)的動(dòng)力學(xué)如下形式：
[0072] Bs=M^(-Fes+^
[0073] 定義跟蹤誤差為：
[0074] e = 9s-0d
[0075] 定義控制量為：
[0076] ts： = Mlu (~Kte ~ K2e) + + Mm&，i
[0077] 其中，KjPK2為正定對(duì)角陣，可得動(dòng)力學(xué)模型：
[0078] e + K.e + K.e^O
[0079 ]式中各分量解耦，通過(guò)選取KjPK2，可以滿足控制需求。
[0080]所述的步驟S3中配置快變子系統(tǒng)的控制律包含：
[0081 ]構(gòu)造最優(yōu)控制性能指標(biāo)函數(shù)：
[0083]其中，Q為半正定加權(quán)對(duì)稱矩陣，R為正定加權(quán)對(duì)稱矩陣，于是，快變子系統(tǒng)的最優(yōu) 控制律可設(shè)計(jì)為：
[0084] tf = -Κ ,ζ, = - R 'B\ Pz,
[0085] 并且P為Ricatti方程的解：
[0086] P!, + 4;Ρ-Ρβ?,Ρ + ?)=〇
[0087] 所述的快變子系統(tǒng)采用線性二次型控制。
[0088] 所述的慢變子系統(tǒng)采用非線性PID控制。
[0089] 本發(fā)明一種空間柔性機(jī)械臂振動(dòng)抑制算法與現(xiàn)有技術(shù)相比具有以下優(yōu)點(diǎn):應(yīng)用奇 異攝動(dòng)理論將系統(tǒng)分解成快慢兩個(gè)獨(dú)立的子系統(tǒng)，簡(jiǎn)化了控制器的設(shè)計(jì)計(jì)算，實(shí)現(xiàn)了對(duì)兩 個(gè)子系統(tǒng)控制器的獨(dú)立設(shè)計(jì)，通過(guò)兩個(gè)控制器的組合得到整個(gè)系統(tǒng)的控制輸入;采用組合 控制方法能夠較好地改善系統(tǒng)的穩(wěn)定控制性能，有效地抑制柔性機(jī)械臂的撓性振動(dòng)，易于 工程實(shí)現(xiàn)和應(yīng)用;本發(fā)明可使柔性機(jī)械臂運(yùn)動(dòng)過(guò)程微振動(dòng)對(duì)中心體運(yùn)動(dòng)和機(jī)械臂各關(guān)節(jié)定 位的影響減小，保證中心剛體姿態(tài)穩(wěn)定的基礎(chǔ)上，有效地抑制了柔性機(jī)械臂的撓性振動(dòng)，顯 著改善系統(tǒng)的姿態(tài)控制性能。
【附圖說(shuō)明】
[0090] 圖1為本發(fā)明一種空間柔性機(jī)械臂振動(dòng)抑制算法的流程圖。
【具體實(shí)施方式】
[0091] 以下結(jié)合附圖，通過(guò)詳細(xì)說(shuō)明一個(gè)較佳的具體實(shí)施例，對(duì)本發(fā)明做進(jìn)一步闡述。
[0092] 在軌服務(wù)柔性機(jī)械臂的中心剛體姿態(tài)動(dòng)力學(xué)方程中除了有剛性中心體慣性特性 效應(yīng)還有各個(gè)柔性機(jī)械臂產(chǎn)生的撓性特性影響耦合部分，另外還包含柔性機(jī)械臂振動(dòng)部分 對(duì)中心剛體作用的累加外力矩部分。
[0093] 如圖1所示，一種空間柔性機(jī)械臂振動(dòng)抑制算法，用于剛?cè)狁詈蠙C(jī)械臂系統(tǒng)中，所 述的剛?cè)狁詈蠙C(jī)械臂系統(tǒng)包含依次連接的中心體、第一柔性機(jī)械臂、第二柔性機(jī)械臂及一 剛性機(jī)械臂，該空間柔性機(jī)械臂振動(dòng)抑制算法包含以下步驟：
[0094] S1、在廣義坐標(biāo)系下，建立剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型。
[0095] 在中心體質(zhì)心位置建立本體坐標(biāo)系Sb，選取廣義坐標(biāo)ν=[Λ' ; < < <朽< W]， 其中，X為中心體平動(dòng)變量，仇為中心體本體坐標(biāo)系相對(duì)慣性坐標(biāo)系的姿態(tài)角，_為與中心 體相連的第一柔性機(jī)械臂關(guān)節(jié)旋轉(zhuǎn)角度，Φ2為第一柔性機(jī)械臂與第二柔性臂之間旋轉(zhuǎn)關(guān) 節(jié)的轉(zhuǎn)角，Φ3為剛性機(jī)械臂相對(duì)于第二柔性機(jī)械臂的旋轉(zhuǎn)角度，^為第一柔性機(jī)械臂的模 態(tài)坐標(biāo)，Τ2為第二柔性機(jī)械臂的模態(tài)坐標(biāo)；
[0096] 設(shè)偏速度列陣為，=[醉Pf Wf町可知 W{ =X;W2 =φ,=ω"·,Ψ3 =φ{(diào) =Ω1；^4 =il；W5 = φ2;Ψ6 =%.

[0103]可得剛性機(jī)械臂的各階(11=1，2，...，7)為：
[0105]得到完整的動(dòng)力學(xué)方程，
[0106] 其中，中心體平動(dòng)方程形式表示為：
[0107] mX - AeiS^mb - - Ae2S^n^t - Ae3S；il3 + ΑΛΡ^ + Ae2P：r2 = Qx
[0108] 中心體轉(zhuǎn)動(dòng)方程形式表示為：
[0109] -SrAA + Jamb + Jb0{ + J,5n^ + Jjli: + B；^ + =
[0110] 第一柔性機(jī)械臂的轉(zhuǎn)動(dòng)方程形式表不為：
[0111] -SlxrAuX + JTM<ab + Jni\ + JnIl(p2 + Jl}n3 + + Η；.,τ2 = Qih
[0112] 第二柔性機(jī)械臂的轉(zhuǎn)動(dòng)方程形式表示為：
[0113] -nTS^TA2eX +nTJT62&b +πν 22π^2 +Πν2,Ω, +nri/：1T1 +Πγ//：2γ2 =ρΩ,
[0114] 剛性機(jī)械臂的轉(zhuǎn)動(dòng)方程形式表示為：
[0115] -S^.AieX. + 4：^. + 4Ω, + Ι^πφ2. + J3^3 + + = g〇s
[0116] 第一柔性機(jī)械臂的振動(dòng)方程形式表不為：
[0117] PiTAicX + HU?, + //,7?, + //；[ηφ-, + //；fΩ3 + (/, + WIX )τ, + Ψ12τ2 + A(r, =
[0118] 第二柔性機(jī)械臂的振動(dòng)方程形式表示為：
[0119] P7A1cX + Hl\mb + Η{τ: Ω, + Η；ι2Πφ2 + Η；???} + + (/, + Wlz )τ2 + Α,τ2 = 0Γ,
[0120] 其中，Jbb為衛(wèi)星相對(duì)整星質(zhì)心的轉(zhuǎn)動(dòng)慣量矩陣，4 = 為衛(wèi)星中心體的姿態(tài)角 速度，內(nèi)鉸處的連體坐標(biāo)系已設(shè)定為礞=氣為入)，以此類推;第一柔性機(jī)械臂、第二柔性 機(jī)械臂級(jí)一剛性機(jī)械臂之間只有一個(gè)旋轉(zhuǎn)關(guān)節(jié)，即有Ω 2=Π 電、Π = [0 0 !]' 表示沿著Z軸旋轉(zhuǎn)。
[0121 ]剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型的標(biāo)準(zhǔn)形式如下：
[0123] 式中，Θ包含廣義坐標(biāo)中中心體平動(dòng)和轉(zhuǎn)動(dòng)六個(gè)變量、三個(gè)機(jī)械臂的旋轉(zhuǎn)關(guān)節(jié)角 度，q包含了二個(gè)柔性機(jī)械臂的振動(dòng)坐標(biāo)，F(xiàn)e為與Θ變量對(duì)應(yīng)的廣義外力，F(xiàn) q為與變量對(duì)應(yīng)的 廣義外力，G為各哥氏力，τ為廣義模態(tài)力，K為剛性矩陣；
[0124] 剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的質(zhì)量矩陣M(0，q)是一正定矩陣，設(shè)它的逆陣為H(0，q)，那 么上式變?yōu)椋?br>[0126]上式可分別寫(xiě)為：
[0129]定義最小剛度系數(shù)為k=min(kii)，定義μ=?Λ，引入新的變量z = kq，貝lj有q = yz， 定義# = . A：,,，將新的變量代入上式得到：
[0132] S2、對(duì)剛?cè)狁詈蠙C(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型中的廣義變量進(jìn)行分解，得到快變參數(shù) 和慢變參數(shù);其中，快變參數(shù)包含柔性機(jī)械臂微振動(dòng)的模態(tài)坐標(biāo)，慢變參數(shù)包含中心體的平 動(dòng)位移、中心體的轉(zhuǎn)動(dòng)姿態(tài)角以及機(jī)械臂各個(gè)關(guān)節(jié)位移量。
[0134] [Mlls(0,0)]-1 = Hlls(0,〇)-Hl2s(0,〇)[H22s(0,0)]-1H21s(0,〇);
[0135] 且(狄冷、"二//i)==((,(設(shè)二=0
[0136] 式中下標(biāo)"s"表示向量處于慢變子系統(tǒng)，即變量在慢時(shí)標(biāo)中的計(jì)算；
[0139]弓丨入快變時(shí)標(biāo)而，定義新的狀態(tài)變量zfi = z_zs，.:rn ，.式中下標(biāo)"f"表示 變量處于快變子系統(tǒng)，于是式(1)可變換為：
[0143]由于慢變時(shí)標(biāo)與快變時(shí)標(biāo)相互獨(dú)立，且在邊界層的區(qū)域μ-0內(nèi)慢變分量可視為常 數(shù)(即i/zs = =0)，此時(shí)，如果令μ = 〇,得到：
[0145] 綜合狀態(tài)變量zfl及上式，可得到以狀態(tài)方程形式描述的快變子系統(tǒng)為：
[0146] zf =Α?ζ? +Β,τ,
[0147] 其中，
[0151] 為快變子系統(tǒng)的控制輸入，用于抑制柔性振動(dòng)，" ?"為對(duì)快變時(shí)標(biāo)ξ求導(dǎo)；剛?cè)?親合機(jī)械臂系統(tǒng)的總控制輸入為T = TS+Tf;原系統(tǒng)的狀態(tài)變量近似為θ = θ3+0(μ)，z = Zs+Zfl +〇(μ)，當(dāng)μ充分小時(shí)，可忽略高階無(wú)窮小項(xiàng)〇(μ)。
[0152] S3、分別針對(duì)快變參數(shù)和慢變參數(shù)建立相應(yīng)的子系統(tǒng)，并配置對(duì)應(yīng)的控制律。
[0153] 配置慢變子系統(tǒng)的控制律包含：
[0154] 設(shè)慢變子系統(tǒng)的動(dòng)力學(xué)如下形式：
[0155] Θ, = (~Fes +ts)
[0156] 由于所考慮的是柔性空間機(jī)械臂載體姿態(tài)與機(jī)械臂各關(guān)節(jié)鉸協(xié)調(diào)運(yùn)動(dòng)的控制，因 此系統(tǒng)的控制輸出為9S，定義系統(tǒng)的期望輸出向量為9 d，定義跟蹤誤差為：
[0157] e = 9s-0d
[0158] 定義控制量為：
[0159 ] = [-K.fi - K2e). + 'F0s + M：ns0d
[0160] 其中，KdPK2為正定對(duì)角陣，可得動(dòng)力學(xué)模型：
[0161 ] e + +
[0162 ]式中各分量解耦，通過(guò)選取KjPK2，可以滿足控制需求。
[0163] 針對(duì)快變子系統(tǒng)，由于(Af，Bf)完全可控，因此可采用最優(yōu)控制理論，即用線性二次 型最優(yōu)控制器(LQR)以實(shí)現(xiàn)快變子系統(tǒng)的控制。配置快變子系統(tǒng)的控制律包含：
[0164] 構(gòu)造最優(yōu)控制性能指標(biāo)函數(shù)：
[0166]其中，Q為半正定加權(quán)對(duì)稱矩陣，R為正定加權(quán)對(duì)稱矩陣，于是，快變子系統(tǒng)的最優(yōu) 控制律可設(shè)計(jì)為：
[0170] S4、將快變子系統(tǒng)的控制律和慢變子系統(tǒng)的控制律進(jìn)行復(fù)合，以定位剛?cè)狁詈蠙C(jī) 械臂系統(tǒng)的位置并進(jìn)行振動(dòng)抑制;快變子系統(tǒng)采用線性二次型控制，用于抑制振動(dòng)，慢變子 系統(tǒng)采用非線性PID控制，用于提高剛體運(yùn)動(dòng)的魯棒性。
[0171] 系統(tǒng)總的控制輸入則為
[0172] τ = Ts+Tf
[0173] 由于所設(shè)計(jì)的線性二次型控制器采用的是全狀態(tài)反饋的形式，而振動(dòng)測(cè)量敏感器 測(cè)得的需要包括衛(wèi)星本體位姿以外，還需要測(cè)量柔性機(jī)械臂振動(dòng)信息。以此實(shí)際操作過(guò)程 中需要采用激光、光纖傳感器等敏感器。
[0174] 盡管本發(fā)明的內(nèi)容已經(jīng)通過(guò)上述優(yōu)選實(shí)施例作了詳細(xì)介紹，但應(yīng)當(dāng)認(rèn)識(shí)到上述的 描述不應(yīng)被認(rèn)為是對(duì)本發(fā)明的限制。在本領(lǐng)域技術(shù)人員閱讀了上述內(nèi)容后，對(duì)于本發(fā)明的 多種修改和替代都將是顯而易見(jiàn)的。因此，本發(fā)明的保護(hù)范圍應(yīng)由所附的權(quán)利要求來(lái)限定。
【主權(quán)項(xiàng)】
1. 一種空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，用于剛?cè)岫U合機(jī)械臂系統(tǒng)中，所述 的剛?cè)岫U合機(jī)械臂系統(tǒng)包含依次連接的中屯、體、第一柔性機(jī)械臂、第二柔性機(jī)械臂及一剛 性機(jī)械臂，該空間柔性機(jī)械臂振動(dòng)抑制算法包含W下步驟： 51、 在廣義坐標(biāo)系下，建立剛?cè)岫U合機(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型； 52、 對(duì)剛?cè)岫U合機(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型中的廣義變量進(jìn)行分解，得到快變參數(shù)和慢 變參數(shù)； 53、 分別針對(duì)快變參數(shù)和慢變參數(shù)建立相應(yīng)的子系統(tǒng)，并配置對(duì)應(yīng)的控制律； 54、 將快變子系統(tǒng)的控制律和慢變子系統(tǒng)的控制律進(jìn)行復(fù)合，W定位剛?cè)岫U合機(jī)械臂 系統(tǒng)的位置并進(jìn)行振動(dòng)抑制。2. 如權(quán)利要求1所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的快變參數(shù)包 含柔性機(jī)械臂微振動(dòng)的模態(tài)坐標(biāo)。3. 如權(quán)利要求1所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的慢變參數(shù)包 含中屯、體的平動(dòng)位移、中屯、體的轉(zhuǎn)動(dòng)姿態(tài)角W及機(jī)械臂各個(gè)關(guān)節(jié)位移量。4. 如權(quán)利要求1所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的步驟S1包 含： 在中屯、體質(zhì)屯、位置建立本體坐標(biāo)系Sb，選取廣義坐標(biāo)扳衫片巧運(yùn)括]，其 中，X為中屯、體平動(dòng)變量，化為中屯、體本體坐標(biāo)系相對(duì)慣性坐標(biāo)系的姿態(tài)角，斬為與中屯、體 相連的第一柔性機(jī)械臂關(guān)節(jié)旋轉(zhuǎn)角度，帶2為第一柔性機(jī)械臂與第二柔性臂之間旋轉(zhuǎn)關(guān)節(jié)的 轉(zhuǎn)角，畢3為剛性機(jī)械臂相對(duì)于第二柔性機(jī)械臂的旋轉(zhuǎn)角度，τι為第一柔性機(jī)械臂的模態(tài)坐 標(biāo)，Τ2為第二柔性機(jī)械臂的模態(tài)坐標(biāo)； 設(shè)偏速度列陣為[聽(tīng)' 嶺r/ W/' r/的]，可知 巧=灰巧:=耗=碼;K =病町=亡1;狀5 =堿;K =知而=柄=〇3 . 利用關(guān)系得到，中屯、體的各階運(yùn)'* (11 = 1，2，..:.，7)為：可得第一柔性機(jī)械臂的各階每* (虹=1，2，...,7)為：可得第二柔性機(jī)械臂的各階( h = 1，2,..,，7)為：得到完整的動(dòng)力學(xué)方程， 其中，中屯、體平動(dòng)方程形式表示為：其中，Jbb為衛(wèi)星相對(duì)整星質(zhì)屯、的轉(zhuǎn)動(dòng)慣量矩陣，或=瓜辟為衛(wèi)星中屯、體的姿態(tài)角速度， 內(nèi)較處的連體坐標(biāo)系已設(shè)定為皆=戰(zhàn)此類推;第一柔性機(jī)械臂、第二柔性機(jī)械臂 級(jí)一剛性機(jī)械臂之間只有一個(gè)旋轉(zhuǎn)關(guān)節(jié)，即有。i=a濟(jì)、化=Π 拓、Π = [0 0 1]/表示沿 著Ζ軸旋轉(zhuǎn)。5. 如權(quán)利要求4所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的剛?cè)岫U合機(jī) 械臂系統(tǒng)的動(dòng)力學(xué)模型的標(biāo)準(zhǔn)形式如下：式中，Θ包含廣義坐標(biāo)中中屯、體平動(dòng)和轉(zhuǎn)動(dòng)六個(gè)變量、Ξ個(gè)機(jī)械臂的旋轉(zhuǎn)關(guān)節(jié)角度，q包 含了二個(gè)柔性機(jī)械臂的振動(dòng)坐標(biāo)，F(xiàn)e為與Θ變量對(duì)應(yīng)的廣義外力，F(xiàn)q為與變量對(duì)應(yīng)的廣義外 力，G為各哥氏力，τ為廣義模態(tài)力，K為剛性矩陣； 剛?cè)岫U合機(jī)械臂系統(tǒng)的質(zhì)量矩陣M(0，q)是一正定矩陣，設(shè)它的逆陣為H(0，q)，那么上 式變?yōu)椋憾x最小剛度系數(shù)為k = min化ii)，定義μ=?Λ，引入新的變量z = kq，則有q =化，定義 兵=,"· /("，將新的變量代入上式得到：6. 如權(quán)利要求5所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的步驟S2中包 含： 令μ充分小，則公??〇簡(jiǎn)化為： [Mils(目，0)]-1 =出 Is(目，0)-出 2s(目，0)[出 2s(目，0)]-1 出 Is(目，0); 且(?"(慶分,"?)二(7。(化灰二 0 式中下標(biāo)V'表示向量處于慢變子系統(tǒng)，即變量在慢時(shí)標(biāo)中的計(jì)算；引入快變時(shí)標(biāo)《=f / '如，定義新的狀態(tài)變量Zf 1 = Z-Zs，5：.。= .，式中下標(biāo)吁"表示變量 處于快變子系統(tǒng)，于是式(1 )可變換為：其中，式(1)表示為：由于慢變時(shí)標(biāo)與快變時(shí)標(biāo)相互獨(dú)立，且在邊界層的區(qū)域μ^ο內(nèi)慢變分量可視為常數(shù) (即?/ζ, / 運(yùn),=0)，此時(shí)，如果令μ = 〇,得至Ij:綜合狀態(tài)變量Zfi及上式，可得到W狀態(tài)方程形式描述的快變子系統(tǒng)為：Tf為快變子系統(tǒng)的控制輸入，用于抑制柔性振動(dòng)，"?"為對(duì)快變時(shí)標(biāo)ξ求導(dǎo)；剛?cè)岫U合機(jī) 械臂系統(tǒng)的總控制輸入為τ = τs+τf;原系統(tǒng)的狀態(tài)變量近似為θ = θs+o(μ)，z = Zs+Zfl+o(μ)， 當(dāng)μ充分小時(shí)，可忽略高階無(wú)窮小項(xiàng)〇(μ)。7. 如權(quán)利要求6所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的步驟S3中配 置慢變子系統(tǒng)的控制律包含： 設(shè)慢變子系統(tǒng)的動(dòng)力學(xué)如下形式：定義跟蹤誤差為： e =目S-目d 定義控制量為：其中，Κι和K2為正定對(duì)角陣，可得動(dòng)力學(xué)模型：式中各分量解禪，通過(guò)選取Κι和拉，可W滿足控制需求。8. 如權(quán)利要求7所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的步驟S3中配 置快變子系統(tǒng)的控制律包含： 構(gòu)造最優(yōu)控制性能指標(biāo)函數(shù)：其中，Q為半正定加權(quán)對(duì)稱矩陣，R為正定加權(quán)對(duì)稱矩陣，于是，快變子系統(tǒng)的最優(yōu)控制 律可設(shè)計(jì)為： tf=-K,:.,=-R、B[Pz, 并且P為化catti方程的解：9. 如權(quán)利要求1所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的快變子系統(tǒng) 采用線性二次型控制。10. 如權(quán)利要求1所述的空間柔性機(jī)械臂振動(dòng)抑制算法，其特征在于，所述的慢變子系 統(tǒng)采用非線性PID控制。
【文檔編號(hào)】G05B13/04GK106094528SQ201610549912
【公開(kāi)日】2016年11月9日
【申請(qǐng)日】2016年7月13日
【發(fā)明人】孫宏麗, 盧山, 侯月陽(yáng), 孫玥, 孫祿君, 王奉文
【申請(qǐng)人】上海航天控制技術(shù)研究所