多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法
【專利摘要】本發(fā)明公開了一種多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，該方法包含：飛行器獲取從當(dāng)前姿態(tài)角轉(zhuǎn)向目標(biāo)姿態(tài)角對(duì)應(yīng)的偏差四元數(shù)；飛行器根據(jù)最大機(jī)動(dòng)角速度，結(jié)合PD控制方法對(duì)偏差四元數(shù)進(jìn)行限幅處理；飛行器根據(jù)已限幅的偏差四元數(shù)分解滾動(dòng)、俯仰、偏航三通道的偏差姿態(tài)角；飛行器采用PD控制方法得出姿態(tài)機(jī)動(dòng)噴氣控制指令。本發(fā)明克服了星體作大角度機(jī)動(dòng)運(yùn)動(dòng)時(shí)歐拉角描述帶來的奇異性；通過對(duì)偏差四元數(shù)的等比例限幅并結(jié)合PD控制規(guī)律對(duì)最大機(jī)動(dòng)角速度進(jìn)行限制，在實(shí)現(xiàn)最優(yōu)路徑的多軸快速姿態(tài)機(jī)動(dòng)控制的同時(shí)保證了機(jī)動(dòng)的安全性。
【專利說明】
多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及衛(wèi)星姿態(tài)機(jī)動(dòng)控制技術(shù)領(lǐng)域，具體設(shè)及一種角速度受限的多軸快速姿 態(tài)機(jī)動(dòng)噴氣控制方法。
【背景技術(shù)】
[0002] -般的對(duì)地觀測(cè)衛(wèi)星對(duì)姿態(tài)機(jī)動(dòng)往往沒有多軸同時(shí)進(jìn)行快速姿態(tài)機(jī)動(dòng)的要求，通 常用歐拉角來描述，并采用路徑規(guī)劃的方式來保證機(jī)動(dòng)的平穩(wěn)性;另外一般衛(wèi)星采用噴氣 作為執(zhí)行機(jī)構(gòu)時(shí)往往采用斜開關(guān)線或相平面控制，由于要兼顧穩(wěn)定控制和機(jī)動(dòng)控制性能， 需要設(shè)置多組參數(shù)，不易調(diào)試。
[0003] 當(dāng)前狀態(tài)下，飛行器一般不僅要求對(duì)地觀測(cè)，為了實(shí)現(xiàn)特定的任務(wù)往往要求在規(guī) 定的較短時(shí)間內(nèi)完成多軸機(jī)動(dòng)。

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

[0004] 本發(fā)明提供一種多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，實(shí)現(xiàn)最優(yōu)路徑的多軸同時(shí)快速 姿態(tài)機(jī)動(dòng)，同時(shí)由于對(duì)機(jī)動(dòng)最大角速度進(jìn)行了限制，保證了飛行器機(jī)動(dòng)過程的安全性。
[0005] 為實(shí)現(xiàn)上述目的，本發(fā)明提供一種多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特點(diǎn)是，該 方法包含：
[0006] 飛行器獲取從當(dāng)前姿態(tài)角轉(zhuǎn)向目標(biāo)姿態(tài)角對(duì)應(yīng)的偏差四元數(shù)；
[0007] 飛行器根據(jù)最大機(jī)動(dòng)角速度，結(jié)合PD控制方法對(duì)偏差四元數(shù)進(jìn)行限幅處理；
[000引飛行器根據(jù)已限幅的偏差四元數(shù)分解滾動(dòng)、俯仰、偏航=通道的偏差姿態(tài)角；
[0009] 飛行器采用PD控制方法得出姿態(tài)機(jī)動(dòng)噴氣控制指令。
[0010] 上述獲取偏差四元數(shù)包含
[0011] 飛行器根據(jù)當(dāng)前=軸姿態(tài)角獲取當(dāng)前四元數(shù)；
[0012]飛行器根據(jù)目標(biāo)姿態(tài)角獲取目標(biāo)四元數(shù)；
[0013] 飛行器根據(jù)當(dāng)前四元數(shù)和目標(biāo)四元數(shù)，通過連續(xù)轉(zhuǎn)動(dòng)的四元數(shù)方法獲得偏差四元 數(shù)。
[0014] 上述獲取當(dāng)前四元數(shù)包含：
[0015] 根據(jù)當(dāng)前姿態(tài)角采用3-1-2轉(zhuǎn)序獲取當(dāng)前四元數(shù)q;
[0016] Cl)
[0017] 式（1)中，盡為飛行器當(dāng)前的滾動(dòng)姿態(tài)角，0為飛行器當(dāng)前的俯仰姿態(tài)角、4為飛行器 當(dāng)前的偏航姿態(tài)角。
[0020]
[001引上述獲取目標(biāo)四元數(shù)包含：[0019] 庶;B日協(xié)盜太值巧田'^-! -9柱皮巧巧日協(xié)而^漸'n J .
(2)
[0021] 式(2)中，為飛行器的滾動(dòng)目標(biāo)姿態(tài)角、0d為飛行器的俯仰目標(biāo)姿態(tài)角、恥!為飛 行器的偏航目標(biāo)姿態(tài)角。
[0022] 上述偏差四元數(shù)dq如式(3):
[0023] 13)
[0024] 式(3)中，q-i為當(dāng)前四元數(shù)的逆；
[002引 q加、qdi、qd2、qd3分別為目標(biāo)四元數(shù)Qd的四個(gè)元素，qd=[qd0 Qdi qd2 qd3]T;
[0026] qo、ql、q2、q3分別為飛行器當(dāng)前四元數(shù)q的四個(gè)元素，q=[qoqlq2q3]T。
[0027] 上述對(duì)偏差四元數(shù)進(jìn)行限幅處理包含：
[0028] 獲取偏差四元數(shù)的矢量部分限幅值dqvlim;
[0029] 偏差四元數(shù)的矢量部分限幅值處理。
[0030] 上述獲取偏差四元數(shù)的矢量部分限幅值包含：
[0031] 記最大機(jī)動(dòng)角速度為W Iim rad/s，記控制的角度增益為kp，記控制的角速度增益 為kd，則巧據(jù)擊（4)巧巧偏差網(wǎng)元掛暇幅值dqvl im: 123456
(4)。 2 上述偏差四元數(shù)的矢量部分限幅值處理包含： 3 取偏差四元數(shù)dq的矢量部分dqvi、dqv2、dqv3中絕對(duì)值最大的量，記為dqvmax; 4 若dqvmax大于偏差四元數(shù)的矢量部分的限幅值dqvlim，則對(duì)偏差四元數(shù)的矢量部 分進(jìn)行限幅處理，限幅后的偏差四元數(shù)的矢量部分如式(5): 5 6 (5)。
[00；3 引
[0039] 上述飛行器根據(jù)已限幅的偏差四元數(shù)分解滾動(dòng)、俯仰、偏航=通道的偏差姿態(tài)角 包含：
[0040] 隨著機(jī)動(dòng)過程推進(jìn)，偏差四元數(shù)逐步接近0,則根據(jù)式(6)簡(jiǎn)化得到滾動(dòng)偏差姿態(tài) 角餐、仰俯偏差姿態(tài)角0_c〇n、偏航偏差姿態(tài)角4_[011的關(guān)系式，單位為rad:
[0041]
[0042] 0_con = -2*dqv2_iim
[0043] i])_con = -2*dqv3_iim (6)。
[0044] 上述飛行器采用PD控制方法得出姿態(tài)機(jī)動(dòng)噴氣控制指令包含：
[0045] 引入控制的S軸姿態(tài)角為滾動(dòng)偏差姿態(tài)角仰俯偏差姿態(tài)角0_c〇n、偏航 偏差姿態(tài)角!lLcon，記為eu_con，引入控制的S軸姿態(tài)角速度為CO bo，角度控制增益為kp，角 速度控制增益為kd，控制指令記為化CP，則得到式(7):
[0046] !"acp = -化 p*eu_con+kd*?bo)*pi/180 (7)
[0047] 那么化CP即為姿態(tài)機(jī)動(dòng)噴氣控制指令，單位為ms，根據(jù)推力器布局分配給相應(yīng)的 推力器實(shí)現(xiàn)控制。
[004引本發(fā)明多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法和現(xiàn)有技術(shù)的飛行器姿態(tài)控制技術(shù)相比， 其優(yōu)點(diǎn)在于，本發(fā)明克服了星體作大角度機(jī)動(dòng)運(yùn)動(dòng)時(shí)歐拉角描述帶來的奇異性；
[0049] 本發(fā)明通過對(duì)偏差四元數(shù)的等比例限幅并結(jié)合PD控制規(guī)律對(duì)最大機(jī)動(dòng)角速度進(jìn) 行限制，在實(shí)現(xiàn)最優(yōu)路徑的多軸快速姿態(tài)機(jī)動(dòng)控制的同時(shí)保證了機(jī)動(dòng)的安全性；
[0050] 本發(fā)明采用PD噴氣控制代替?zhèn)鹘y(tǒng)的斜開關(guān)線或相平面控制，算法簡(jiǎn)單，控制參數(shù) 減少，易于調(diào)試。
【附圖說明】
[0051 ]圖1為本發(fā)明多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法的控制原理圖；
[0052] 圖2為本發(fā)明多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法的流程圖。
【具體實(shí)施方式】
[0053] W下結(jié)合附圖，進(jìn)一步說明本發(fā)明的具體實(shí)施例。
[0054] 如圖1所示，本發(fā)明提供一種多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，該控制方法的原理 包含:通過飛行器目標(biāo)姿態(tài)和當(dāng)前=軸姿態(tài)獲取偏差四元數(shù)，對(duì)獲取的偏差四元數(shù)進(jìn)行等 比例限幅處理，然后根據(jù)經(jīng)過限幅處理的偏差四元數(shù)結(jié)算得到=通道控制姿態(tài)，=通道控 制姿態(tài)包含滾動(dòng)偏差姿態(tài)角、仰俯偏差姿態(tài)角、偏航偏差姿態(tài)角。=通道控制姿態(tài)控制飛行 器的姿態(tài)角度，結(jié)合控制角速度得到飛行器的控制指令，將控制指令發(fā)送至飛行器的執(zhí)行 機(jī)構(gòu)，執(zhí)行機(jī)構(gòu)控制飛行器進(jìn)行姿態(tài)機(jī)動(dòng)。在機(jī)動(dòng)過程中根據(jù)星體動(dòng)力學(xué)確定飛行器的當(dāng) 前姿態(tài)，作為下一次飛行器姿態(tài)機(jī)動(dòng)的基礎(chǔ)。
[0055] 如圖2所示，公開了一種多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法的實(shí)施例，該控制方法具 體包含W下步驟：
[0056] S1、飛行器獲取從當(dāng)前姿態(tài)角轉(zhuǎn)向目標(biāo)姿態(tài)角對(duì)應(yīng)的偏差四元數(shù)。
[0057] Sl. 1、飛行器根據(jù)當(dāng)前S軸姿態(tài)角獲取當(dāng)前四元數(shù)。
[005引根據(jù)當(dāng)前姿態(tài)角采用3-1-2轉(zhuǎn)序獲取當(dāng)前四元數(shù)q:
[0化9] (1)
[0060] 式（1)中，?>為飛行器當(dāng)前的滾動(dòng)姿態(tài)角，0為飛行器當(dāng)前的俯仰姿態(tài)角、4為飛行器 當(dāng)前的偏航姿態(tài)角。
[0061] 其中，3-1-2轉(zhuǎn)序指:先繞偏航軸轉(zhuǎn)動(dòng)，再繞滾動(dòng)軸轉(zhuǎn)動(dòng)，最后繞俯仰軸轉(zhuǎn)動(dòng)。
[0062] Sl. 2、飛行器根據(jù)目標(biāo)姿態(tài)角獲取目標(biāo)四元數(shù)。
[0063] 根據(jù)目標(biāo)姿態(tài)角采用3-1-2轉(zhuǎn)序獲取目標(biāo)四元數(shù)qd:
[0064] (2)
[00化]式（2)中，鮮/為飛行器的滾動(dòng)目標(biāo)姿態(tài)角、0d為飛行器的俯仰目標(biāo)姿態(tài)角、恥!為飛 行器的偏航目標(biāo)姿態(tài)角。
[0066] SI. 3、飛行器根據(jù)當(dāng)前四元數(shù)和目標(biāo)四元數(shù)，通過連續(xù)轉(zhuǎn)動(dòng)的四元數(shù)方法獲得偏 差四元數(shù)dq。
[0067] 連續(xù)轉(zhuǎn)動(dòng)四元數(shù)方法是指:多個(gè)坐標(biāo)變換的四元數(shù)轉(zhuǎn)換關(guān)系，如從坐標(biāo)A變換到坐 標(biāo)B的四元數(shù)記為q，從坐標(biāo)B變換到坐標(biāo)C的四元數(shù)記為q，從坐標(biāo)A變換到坐標(biāo)C的四元數(shù)為 q"，那么g "=去致g
[006引偏差四元數(shù)變量可定義為dq，dq=[dq0 dqvi dqv2 dqv3]T，偏差四元數(shù)變量的矢量 部分dqv=[dql dq2 dq3]T。
[0069] 獲得的偏差四元數(shù)dq如式(3): 1
[0071] 式(3)中，q-i為當(dāng)前四元數(shù)的逆； (3)
[0072] q加、qdi、qd2、qd3分別為目標(biāo)四元數(shù)qd的四個(gè)元素，qd=[qd0 qdi qd2 qd3]T;
[0073] qo、ql、q2、q3分別為飛行器當(dāng)前四元數(shù)q的四個(gè)元素，q=[qoqlq2q3]T。
[0074] S2、飛行器根據(jù)最大機(jī)動(dòng)角速度，結(jié)合PD控制方法對(duì)偏差四元數(shù)進(jìn)行限幅處理。運(yùn) 里PD控制方法是指:根據(jù)姿態(tài)角偏差和角速度偏差求解噴氣指令，詳見下文中式(7)。
[00巧]S2.1、獲取偏差四元數(shù)的矢量部分限幅值dqvlim。
[0076] 記最大機(jī)動(dòng)角速度為CO Iim rad/s，記控制的角度增益為kp，記控制的角速度增益 為kd，贓柏舊^化而m二數(shù)的矢量部分限幅值dqvl im:
[0077] (牛)。
[0078] S2.2、偏差四元數(shù)的矢量部分限幅值處理。
[0079] 取偏差四元數(shù)dq的矢量部分dqvi、dqv2、dqv3中絕對(duì)值最大的量，記為dqvmax。
[0080] 若dqvmax大于偏差四元數(shù)的矢量部分的限幅值dqvlim，則對(duì)偏差四元數(shù)的矢量部 分進(jìn)行服幅々h巧-服幅巨的偏単師擊掀的告量部分如式（5):
[0081]
[0082]
[0083] (5)。
[0084] S3、飛行器根據(jù)已限幅的偏差四元數(shù)分解滾動(dòng)、俯仰、偏航=通道的偏差姿態(tài)角。
[0085] 隨著機(jī)動(dòng)過程推進(jìn)，偏差四元數(shù)逐步接近0,則根據(jù)式(6)簡(jiǎn)化得到滾動(dòng)偏差姿態(tài) 角癸L(fēng). CfWI仰俯偏差姿杰角0 con、偏航偏差姿態(tài)角IlLcon的關(guān)系式，單位為rad:
[0086]
[0087] 0_con = -2*dqv2_iim [008引 i])_con = -2*dqv3_iim (6)。
[0089] S4、飛行器采用PD控制方法得出姿態(tài)機(jī)動(dòng)噴氣控制指令。
[0090] 引入控制的S軸姿態(tài)角為滾動(dòng)偏差姿態(tài)角癸.___ CAfj、仰俯偏差姿態(tài)角e_con、偏航 偏差姿態(tài)角!lLcon，記為eu_con，引入控制的S軸姿態(tài)角速度為CO bo，角度控制增益為kp，角 速度控制增益為kd，控制指令記為化CP，則得到式(7):
[0091] !"acp = -化 p*eu_con+kd*?bo)*pi/180 (7)
[0092] 那么化CP即為姿態(tài)機(jī)動(dòng)噴氣控制指令，單位為ms，根據(jù)推力器布局分配給相應(yīng)的 推力器實(shí)現(xiàn)控制。
[0093] 本發(fā)明算法簡(jiǎn)單，采用基于偏差四元數(shù)的PD控制方法，克服了星體作大角度機(jī)動(dòng) 運(yùn)動(dòng)時(shí)歐拉角描述帶來的奇異性;通過對(duì)偏差四元數(shù)的等比例限幅并結(jié)合PD控制規(guī)律對(duì)最 大機(jī)動(dòng)角速度進(jìn)行限制，在實(shí)現(xiàn)最優(yōu)路徑的多軸快速姿態(tài)機(jī)動(dòng)控制的同時(shí)保證了機(jī)動(dòng)的安 全性;采用PD噴氣控制，算法簡(jiǎn)單，控制參數(shù)易于調(diào)試。
[0094]盡管本發(fā)明的內(nèi)容已經(jīng)通過上述優(yōu)選實(shí)施例作了詳細(xì)介紹，但應(yīng)當(dāng)認(rèn)識(shí)到上述的 描述不應(yīng)被認(rèn)為是對(duì)本發(fā)明的限制。在本領(lǐng)域技術(shù)人員閱讀了上述內(nèi)容后，對(duì)于本發(fā)明的 多種修改和替代都將是顯而易見的。因此，本發(fā)明的保護(hù)范圍應(yīng)由所附的權(quán)利要求來限定。
【主權(quán)項(xiàng)】
1. 一種多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，該方法包含： 飛行器獲取從當(dāng)前姿態(tài)角轉(zhuǎn)向目標(biāo)姿態(tài)角對(duì)應(yīng)的偏差四元數(shù)； 飛行器根據(jù)最大機(jī)動(dòng)角速度，結(jié)合PD控制方法對(duì)偏差四元數(shù)進(jìn)行限幅處理； 飛行器根據(jù)已限幅的偏差四元數(shù)分解滾動(dòng)、俯仰、偏航三通道的偏差姿態(tài)角； 飛行器采用PD控制方法得出姿態(tài)機(jī)動(dòng)噴氣控制指令。2. 如權(quán)利要求1所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述獲取偏差四 元數(shù)包含 飛行器根據(jù)當(dāng)前三軸姿態(tài)角獲取當(dāng)前四元數(shù)； 飛行器根據(jù)目標(biāo)姿態(tài)角獲取目標(biāo)四元數(shù)； 飛行器根據(jù)當(dāng)前四元數(shù)和目標(biāo)四元數(shù)，通過連續(xù)轉(zhuǎn)動(dòng)的四元數(shù)方法獲得偏差四元數(shù)。3. 如權(quán)利要求2所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述獲取當(dāng)前四 元數(shù)包含： 根據(jù)當(dāng)前姿態(tài)角采用3-1-2轉(zhuǎn)序獲取當(dāng)前四元數(shù)q;(1) 式（1)中，供為飛行器當(dāng)前的滾動(dòng)姿態(tài)角，Θ為飛行器當(dāng)前的俯仰姿態(tài)角、Φ為飛行器當(dāng)前 的偏航姿態(tài)角。4. 如權(quán)利要求2所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述獲取目標(biāo)四 元數(shù)包含： 根據(jù)目標(biāo)姿態(tài)角采用3-1-2轉(zhuǎn)序獲取目標(biāo)四元數(shù)qd;(2) 式(2)中，/為飛行器的滾動(dòng)目標(biāo)姿態(tài)角、0d為飛行器的俯仰目標(biāo)姿態(tài)角、ιΜ為飛行器 的偏航目標(biāo)姿態(tài)角。5. 如權(quán)利要求2所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述偏差四元數(shù) dq如式(3): (3) 式(3)中，q^1為當(dāng)前四元數(shù)的逆； qd〇、qdi、qd2、qd3分別為目標(biāo)四元數(shù)qd的四個(gè)元素，qd=[qd〇 qdi qd2 qd3]T; 9〇、91、92、93分別為飛行器當(dāng)前四元數(shù)9的四個(gè)元素，9=[9()9192 93]1'。6. 如權(quán)利要求1所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述對(duì)偏差四元 數(shù)進(jìn)行限幅處理包含： 獲取偏差四元數(shù)的矢量部分限幅值dqvlim; 偏差四元數(shù)的矢量部分限幅值處理。7. 如權(quán)利要求6所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述獲取偏差四 元數(shù)的矢量部分限幅值包含： 記最大機(jī)動(dòng)角速度為ω lim rad/s，記控制的角度增益為kp，記控制的角速度增益為 kd，則根據(jù)式(4)獲取偏差四元數(shù)限幅值dqv 1 im:(4 5 〇8. 如權(quán)利要求7所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述偏差四元數(shù) 的矢量部分限幅值處理包含： 取偏差四元數(shù)dq的矢量部分dqvl、dqv2、dqv3中絕對(duì)值最大的量，記為dq? ax; 若dq?ax大于偏差四元數(shù)的矢量部分的限幅值dqvl im，則對(duì)偏差四元數(shù)的矢量部分進(jìn)行 限幅處理，限幅后的偏差四元數(shù)的矢量部分如式(5):9. 如權(quán)利要求8所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述飛行器根據(jù) 已限幅的偏差四元數(shù)分解滾動(dòng)、俯仰、偏航三通道的偏差姿態(tài)角包含： 隨著機(jī)動(dòng)過程推進(jìn)，偏差四元數(shù)逐步接近〇,則根據(jù)式（6)簡(jiǎn)化得到滾動(dòng)偏差姿態(tài)角 辦-俯偏差姿態(tài)角9_con、偏航偏差姿態(tài)角力_。〇11的關(guān)系式，單位為rad:Θ-con = -2 氺 dqV2-lim Φ_οοη = -2^dqv3_iim (6) 〇10.如權(quán)利要求1或9所述的多軸快速姿態(tài)機(jī)動(dòng)噴氣控制方法，其特征在于，所述飛行器 采用ro控制方法得出姿態(tài)機(jī)動(dòng)噴氣控制指令包含： 弓丨入控制的三軸姿態(tài)角為滾動(dòng)偏差姿態(tài)角、仰俯偏差姿態(tài)角9_con、偏航偏差 姿態(tài)角Φ_(3〇η，記為eu_con，引入控制的三軸姿態(tài)角速度為cobo,角度控制增益為kp，角速度 控制增益為kd，控制指令記為Tacp，則得到式(7): Tacp = _(kp*eu_con+kd* ω bo)*pi/180 (7) 那么Tacp即為姿態(tài)機(jī)動(dòng)噴氣控制指令，單位為ms，根據(jù)推力器布局分配給相應(yīng)的推力 器實(shí)現(xiàn)控制。
【文檔編號(hào)】G05D1/08GK105955283SQ201610370253
【公開日】2016年9月21日
【申請(qǐng)日】2016年5月30日
【發(fā)明人】張肖, 王獻(xiàn)忠, 張國柱, 張麗敏, 盧翔, 胡良軍, 程顥, 郭雯婷
【申請(qǐng)人】上海航天控制技術(shù)研究所