[發(fā)明專利]一種基于雙環(huán)PD控制算法控制的四旋翼無(wú)人機(jī)著陸方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201911066729.8
• 申請(qǐng)日: 2019-11-04
• 公開(kāi)（公告）號(hào): CN110673623B
• 公開(kāi)（公告）日: 2022-11-22
• 發(fā)明（設(shè)計(jì)）人: 宋志強(qiáng);方武;劉孝趙
• 申請(qǐng)（專利權(quán)）人: 蘇州經(jīng)貿(mào)職業(yè)技術(shù)學(xué)院
• 主分類號(hào): G05D1/08
• 分類號(hào): G05D1/08;G05D1/10
• 代理公司: 西安研創(chuàng)天下知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 61239
• 代理人: 郭璐

1.一種基于雙環(huán)PD控制算法控制的四旋翼無(wú)人機(jī)著陸方法，其特征在于，包括以下步驟：

步驟一：建立四旋翼無(wú)人機(jī)MIMO系統(tǒng)的動(dòng)力學(xué)模型；

步驟二：通過(guò)步驟一的動(dòng)力學(xué)模型設(shè)計(jì)外環(huán)位置控制律u₁，以實(shí)現(xiàn)x→x_d，y→y_d，z_d→0，z→z_d；

步驟三：根據(jù)步驟一的動(dòng)力學(xué)模型設(shè)計(jì)內(nèi)環(huán)姿態(tài)控制律，使θ→θ_d，ψ→ψ_d，φ→φ_d，φ_d→0；

步驟四：分別利用位置控制律u₁和姿態(tài)控制律控制的位置子系統(tǒng)和姿態(tài)子系統(tǒng)控制四旋翼無(wú)人機(jī)著陸時(shí)的位置和姿態(tài)，實(shí)現(xiàn)四旋翼無(wú)人機(jī)的自主著陸；

其中：(x,y,z)為UAV質(zhì)心在慣性坐標(biāo)系中的位置坐標(biāo)，(θ,ψ,φ)為無(wú)人機(jī)三個(gè)姿態(tài)的歐拉角，分別為俯仰角、偏航角和滾轉(zhuǎn)角，(x_d，y_d，z_d)為四旋翼無(wú)人機(jī)著陸時(shí)的位置坐標(biāo)，(θ_d,ψ_d,φ_d)為四旋翼無(wú)人機(jī)著陸時(shí)的三個(gè)姿態(tài)的歐拉角；

步驟一所述的四旋翼無(wú)人機(jī)MIMO系統(tǒng)的動(dòng)力學(xué)模型具體為：

其中：(x,y,z)為UAV質(zhì)心在慣性坐標(biāo)系中的位置坐標(biāo)，為無(wú)人機(jī)三個(gè)姿態(tài)的歐拉角，分別為俯仰角、偏航角和滾轉(zhuǎn)角，u₂為俯仰角θ的控制律；u₃為偏航角ψ的控制律；u₄為滾轉(zhuǎn)角的控制律，g為重力加速度，l為無(wú)人機(jī)半徑長(zhǎng)度，m為UAV之負(fù)載總質(zhì)量，I_i為圍繞每個(gè)軸之轉(zhuǎn)動(dòng)慣量，K_i為阻力系數(shù)，為UAV質(zhì)心位置一階導(dǎo)數(shù)，分別代表四旋翼無(wú)人機(jī)在x、y、z方向的線速度，為四旋翼無(wú)人機(jī)在飛行時(shí)三個(gè)姿態(tài)歐拉角的一階導(dǎo)數(shù)，分別代表四旋翼無(wú)人的歐拉角角速度、偏航角角速度、滾轉(zhuǎn)角角速度，為UAV質(zhì)心位置二階導(dǎo)數(shù)，為四旋翼無(wú)人機(jī)在飛行時(shí)三個(gè)姿態(tài)歐拉角的二階導(dǎo)數(shù)；

步驟二所述位置控制律u₁設(shè)計(jì)的具體步驟為：

S1.由公式(1)定義：

S2.對(duì)于第一個(gè)位置子系統(tǒng)采用基于前饋補(bǔ)償?shù)腜D控制算法設(shè)計(jì)控制律:

其中：為四旋翼無(wú)人機(jī)飛行在某一位置時(shí)，該點(diǎn)在x軸上的加速度，為前饋補(bǔ)充項(xiàng)，為PD控制，即比例、微分控制；令x_e＝x-x_d，則則第一個(gè)位置子系統(tǒng)可化簡(jiǎn)為k_p1為x分量比例系數(shù)、k_d1為x分量微分系數(shù)、x_e為無(wú)人機(jī)x方向的誤差，根據(jù)二階系統(tǒng)Hurwitz判據(jù)，需要滿足k_p10，取k_p1＝4.0，k_d1＝4.0；

S3.對(duì)于第二個(gè)位置子系統(tǒng)采用基于前饋補(bǔ)償?shù)腜D控制算法設(shè)計(jì)控制律：

其中：為四旋翼無(wú)人機(jī)飛行在某一位置時(shí)，該點(diǎn)在y軸上的加速度，為前饋補(bǔ)充項(xiàng)，為PD控制，即比例、微分控制；令y_e＝y(tǒng)-y_d，則則第二個(gè)位置子系統(tǒng)可化簡(jiǎn)為k_p2為y分量比例系數(shù)、k_d2為y分量微分系數(shù)、y_e為無(wú)人機(jī)y方向的誤差，根據(jù)二階系統(tǒng)Hurwitz判據(jù)，需要滿足k_p20，要使系統(tǒng)穩(wěn)定，取k_p2＝4.0，k_d2＝4.0；

S4.對(duì)于第三個(gè)位置子系統(tǒng)采用基于前饋補(bǔ)償?shù)腜D控制算法設(shè)計(jì)控制律：

其中：為四旋翼無(wú)人機(jī)飛行在某一位置時(shí)，該點(diǎn)在z軸上的加速度，為前饋補(bǔ)充項(xiàng)，為PD控制，即比例、微分控制；令z_e＝z-z_d，則則第三個(gè)位置子系統(tǒng)可化簡(jiǎn)為k_p3為z分量比例系數(shù)、k_d3為z分量微分系數(shù)、z_e為無(wú)人機(jī)z方向的誤差，根據(jù)二階系統(tǒng)Hurwitz判據(jù)，需要滿足k_p30，要使系統(tǒng)穩(wěn)定，取k_p3＝4.0，k_d3＝4.0；

S5.假定滿足控制律公式(3)-(5)所需要的姿態(tài)角度為θ_d和ψ_d，由公式(2)可得:

由u_1z＝u₁cosφcosψ_d，可得將其代入公式(6)可得:

其中：若X1時(shí)，取θ_d＝π/2；若X-1時(shí)，取θ_d＝-π/2。