[發(fā)明專利]一種基于微分平坦特性的無人機(jī)軌跡跟蹤方法

有效

• 申請?zhí)枺?/td>: 202011183904.4
• 申請日: 2020-10-29
• 公開（公告）號: CN112241125B
• 公開（公告）日: 2021-08-13
• 發(fā)明（設(shè)計(jì)）人: 薛若宸;戴荔;夏元清;張金會(huì);孫中奇;崔冰;翟弟華;閆莉萍;鄒偉東;郭澤華;詹玉峰;夏吟秋
• 申請（專利權(quán)）人: 北京理工大學(xué)
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 北京理工大學(xué)專利中心 11120
• 代理人: 高會(huì)允

1.一種基于微分平坦特性的無人機(jī)軌跡跟蹤方法，其特征在于，包括以下步驟：

步驟一、針對無人機(jī)系統(tǒng)進(jìn)行建模，并通過設(shè)置虛擬輸入將無人機(jī)系統(tǒng)的垂直運(yùn)動(dòng)與水平運(yùn)動(dòng)解耦；

步驟二、根據(jù)無人機(jī)系統(tǒng)的微分平坦性質(zhì)，將無人機(jī)系統(tǒng)從六自由度、四輸入的欠驅(qū)動(dòng)系統(tǒng)轉(zhuǎn)換為四自由度、四輸入的全驅(qū)動(dòng)系統(tǒng)，并求解出姿態(tài)內(nèi)環(huán)的角度期望值；

步驟三、根據(jù)步驟一解耦后的無人機(jī)系統(tǒng)作為實(shí)際系統(tǒng)，另外設(shè)置參考系統(tǒng)與誤差系統(tǒng)，針對誤差系統(tǒng)的水平運(yùn)動(dòng)通道進(jìn)行基于模型預(yù)測控制的軌跡跟蹤；針對誤差系統(tǒng)的垂直運(yùn)動(dòng)通道，基于多面體微分包含理論，實(shí)時(shí)求解終端懲罰矩陣，保證系統(tǒng)穩(wěn)定；

所述步驟一，針對無人機(jī)系統(tǒng)進(jìn)行建模，并通過設(shè)置虛擬輸入將無人機(jī)系統(tǒng)的垂直運(yùn)動(dòng)與水平運(yùn)動(dòng)解耦，具體為：

首先針對無人機(jī)系統(tǒng)構(gòu)建地面坐標(biāo)系和機(jī)體坐標(biāo)系；

所述地面坐標(biāo)系固定于地球表面，坐標(biāo)系原點(diǎn)O_E固定于地面上飛行器的起飛點(diǎn)，O_EX_E軸指向飛行器指定的飛行方向，O_EZ_E軸鉛垂向下，O_EY_E軸垂直O(jiān)_EX_EZ_E平面，按右手定則確定；

機(jī)體坐標(biāo)系固定在無人機(jī)的機(jī)體上，并隨無人機(jī)運(yùn)動(dòng)，原點(diǎn)O_B在無人機(jī)質(zhì)心，O_BX_B軸在無人機(jī)對稱平面內(nèi)，平行于無人機(jī)前后旋翼的連接線，指向前；豎軸O_BZ_B平行于左右旋翼連線，指向右為正方向；O_BY_B與O_BX_BZ_B平面垂直，方向按照右手定則確定；

利用無人機(jī)的三個(gè)姿態(tài)角度俯仰角θ、滾轉(zhuǎn)角φ和偏航角ψ計(jì)算地面坐標(biāo)系與機(jī)體坐標(biāo)系的轉(zhuǎn)換矩陣R：

所述無人機(jī)為四旋翼飛行器，所述四旋翼飛行器是一個(gè)剛體且對稱，在運(yùn)動(dòng)中其質(zhì)量不變；所述地面坐標(biāo)系為慣性坐標(biāo)系，視地球表面為一平面，且重力加速度不隨高度變化；不計(jì)入地球公轉(zhuǎn)與自轉(zhuǎn)對所述四旋翼飛行器的影響；四旋翼飛行器的形狀與質(zhì)量關(guān)于其幾何對稱軸對稱；四旋翼飛行器旋翼電機(jī)所提供的升力與轉(zhuǎn)速成正比，且不計(jì)旋翼的螺旋效應(yīng)；四旋翼飛行器在運(yùn)動(dòng)過程中偏航角ψ＝0；

在機(jī)體坐標(biāo)系下，所述四旋翼飛行器四個(gè)旋翼產(chǎn)生的升力為F_i,i∈{1,2,3,4}，F(xiàn)₁～F₄分別為第1～第4旋翼產(chǎn)生的升力，四個(gè)旋翼產(chǎn)生的力矩為M_i,i∈{1,2,3,4}，M₁～M₄分別為第1～第4旋翼產(chǎn)生的力矩；所述四旋翼飛行器升力U₁、橫滾矩U₂、俯仰矩U₃均由四個(gè)旋翼升力提供，而偏航力矩U₄由旋翼產(chǎn)生的力矩提供；設(shè)置無人機(jī)的四個(gè)輸入分別為

其中l(wèi)為四旋翼飛行器的基體坐標(biāo)系中，旋翼中心線到O_BY_B的距離；

根據(jù)牛頓第二定律與動(dòng)量矩定律得到如下無人機(jī)系統(tǒng)模型

其中m是無人機(jī)的質(zhì)量；g是重力加速度；r為無人機(jī)重心在地面坐標(biāo)系下的位置，為r的二階導(dǎo)數(shù)；i_z＝[0 0 1]^T為方向向量；J為轉(zhuǎn)動(dòng)慣量矩陣；ω＝[p,q,r]為機(jī)體坐標(biāo)系下無人機(jī)角速度，p、q、r分別為無人機(jī)角速度在機(jī)體坐標(biāo)系下三軸分量，為ω的一階導(dǎo)數(shù)；τ為控制無人機(jī)的合外力矩；

將公式(3)、(4)展開為以下解析形式無人機(jī)動(dòng)力學(xué)方程為

其中(x、y、z)為四旋翼飛行器機(jī)體在地面坐標(biāo)系下的位置坐標(biāo)；I_x、I_y、I_z分別為四旋翼飛行器機(jī)體相對于機(jī)體坐標(biāo)系三軸的轉(zhuǎn)動(dòng)慣量；

定義第一虛擬控制量u_x和第二虛擬控制量u_y；

u_x＝cosψsinθcosφ+sinψsinφ (6)

u_y＝sinψsinθcosφ-cosψsinφ (7)

通過設(shè)置第一虛擬控制量u_x和第二虛擬控制量u_y，將無人機(jī)系統(tǒng)的垂直運(yùn)動(dòng)與水平運(yùn)動(dòng)解耦，得到四旋翼飛行器的動(dòng)力學(xué)空間狀態(tài)方程為：

所述步驟二具體為：設(shè)置微分平坦輸出包括四個(gè)，分別為第一輸出Z₁＝x，第二輸出Z₂＝y(tǒng)，第三輸出Z₃＝z，第四輸出Z₄＝ψ；

則U₁，u_x，u_y表示為

俯仰角θ、滾轉(zhuǎn)角φ由微分平坦輸出Z_i,i∈{1,2,3,4}及其有限階導(dǎo)數(shù)表示

解得滾轉(zhuǎn)角期望值φ_r和俯仰角期望值θ_r用于姿態(tài)內(nèi)環(huán)的控制，具體表達(dá)式為

φ_r＝arcsin(u_r,xsinψ_r-u_r,ycosψ_r) (12)

其中ψ_r為偏航角期望值，其數(shù)值為0；u_r,x為水平運(yùn)動(dòng)通道內(nèi)的第一虛擬參考輸入，u_r,y為水平運(yùn)動(dòng)通道內(nèi)的第二虛擬參考輸入，u_r,z為垂直運(yùn)動(dòng)通道的參考輸入。