[發(fā)明專利]六旋翼無人機(jī)的自動容錯姿態(tài)控制方法

有效

• 申請?zhí)枺?/td>: 201810460050.6
• 申請日: 2018-05-15
• 公開（公告）號: CN108614573B
• 公開（公告）日: 2021-08-20
• 發(fā)明（設(shè)計）人: 常亮;蔡星;孫培
• 申請（專利權(quán)）人: 上海擴(kuò)博智能技術(shù)有限公司
• 主分類號: G05D1/08
• 分類號: G05D1/08;G05D1/10
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種六旋翼無人機(jī)的自動容錯姿態(tài)控制方法，所述六旋翼無人機(jī)的每一電機(jī)具有相對于水平面沿一方傾斜安裝角α，沿另一方向的傾斜安裝角β，其特征在于，包括如下步驟：

步驟S1：采集六旋翼無人機(jī)上多個電機(jī)的電流和/或電壓，根據(jù)所述電流和/或電壓判斷每一所述電機(jī)是否存在故障；

步驟S2：當(dāng)判斷出一所述電機(jī)為故障電機(jī)時，關(guān)閉另一與所述故障電機(jī)相鄰且轉(zhuǎn)動方向相反的電機(jī)；

步驟S3：根據(jù)所述故障電機(jī)所驅(qū)動的槳翼i的序號選擇相對應(yīng)的映射矩陣R_i，并根據(jù)所述映射矩陣R_i將輸入姿態(tài)角轉(zhuǎn)換為控制姿態(tài)角，i為大于等于0小于等于6的自然數(shù)；

步驟S4：將所述控制姿態(tài)角輸入所述六旋翼無人機(jī)的飛行控制模塊以控制所述六旋翼無人機(jī)飛行；

在所述步驟S1之前還包括如下步驟：

步驟M1：建立機(jī)體坐標(biāo)系F_B，F(xiàn)_B＝{O_B，(x_B，y_B，z_B)}，其中，O_B為機(jī)體的中心，z_B、x_B、y_B分別為六旋翼無人機(jī)的Z軸、X軸、Y軸；z_B表示六旋翼無人機(jī)在平衡狀態(tài)時，垂直于地面的方向，x_B表示穿過所述機(jī)體的中心沿所述機(jī)體的機(jī)頭延伸的方向，y_B表示垂直于z_B和x_B的方向；

在電機(jī)i水平安裝時，建立初始電機(jī)坐標(biāo)系，在所述初始電機(jī)坐標(biāo)系中z_C為Z軸，表示電機(jī)i的軸線方向且垂直于地面的方向，x_C為X軸，表示穿過所述電機(jī)i的中心沿機(jī)臂延伸的方向，y_C為Y軸，表示垂直于z_C和x_C形成平面的方向；

步驟M2：對于每一電機(jī)i，當(dāng)該電機(jī)i驅(qū)動漿翼i順時針轉(zhuǎn)動時，將該電機(jī)i繞x_C軸逆時針旋轉(zhuǎn)α_i角度以及繞y_C軸逆時針旋轉(zhuǎn)β_i角度；當(dāng)該電機(jī)i驅(qū)動漿翼i逆時針轉(zhuǎn)動時，將該電機(jī)i繞x_C軸順時針旋轉(zhuǎn)α_i角度以及繞y_C軸逆時針旋轉(zhuǎn)β_i角度；在本實(shí)施例，所述逆時針旋轉(zhuǎn)和所述順時針旋轉(zhuǎn)的觀測位置位于所述六旋翼無人機(jī)的外側(cè)；

步驟M3：對于電機(jī)i，建立旋轉(zhuǎn)后電機(jī)坐標(biāo)系其中，i表示電機(jī)的序號，i為大于0小于等于6的自然數(shù)，表示電機(jī)i的中心，為電機(jī)i的Z軸、X軸、Y軸；為Z軸，表示電機(jī)i的軸線方向，為X軸，表示穿過所述電機(jī)i的中心與機(jī)臂延伸的方向呈β_i角度的方向，為Y軸，表示垂直于和的方向；

步驟M4：將每一軸與z_B軸之間的夾角通過傾斜安裝角α、β來表示，由于第i槳翼的z_pi軸與該翼的旋轉(zhuǎn)軸的軸向同方向，第i槳翼的z_pi軸與機(jī)體坐標(biāo)系z_B軸之間的旋轉(zhuǎn)關(guān)系為，

z_pi＝R_y(β_i)R_x(α_i)z_B

其中，R_y(β_i)表示y_B方向的旋轉(zhuǎn)矩陣，R_x(α_i)表示x_B方向的旋轉(zhuǎn)矩，分別表示如下：

當(dāng)z_B＝[0 0 1]的時候，能夠得到：

矩陣的第三個元素；

步驟M5：第i槳翼會產(chǎn)生沿z_pi方向的推力f_i和轉(zhuǎn)矩τ_i，在些標(biāo)系中可以表示為，

其中，k_fi、k_τi為與槳翼物理特性相關(guān)的常系數(shù)，kfi為推力與轉(zhuǎn)速的關(guān)系參數(shù)，k_τi為力矩與轉(zhuǎn)速的關(guān)系參數(shù)，ω_i為每個槳翼的轉(zhuǎn)速，+表示該槳翼的旋轉(zhuǎn)方向?yàn)轫槙r針，-表示該槳翼的旋轉(zhuǎn)方向?yàn)槟鏁r針；

六旋翼無人機(jī)的所有能夠轉(zhuǎn)動的螺旋槳所產(chǎn)生的總推力和總扭矩τ_c在地面坐標(biāo)系下可以表示為

其中，p_i表示原點(diǎn)在機(jī)體坐標(biāo)系F_B中的位置，u＝[ω₁|ω₁|，ω₂|ω₂|…，ω₆|ω₆|]，F(xiàn)₁為控制輸入u到施加到無人機(jī)的推力的轉(zhuǎn)換矩陣，F(xiàn)₂為控制輸入u到施加到無人機(jī)的轉(zhuǎn)矩的轉(zhuǎn)換矩陣，n為能夠轉(zhuǎn)動的電機(jī)的總數(shù)，n小于等于i，R為地面坐標(biāo)系與旋轉(zhuǎn)后電機(jī)坐標(biāo)系之間的轉(zhuǎn)換矩陣，表示3×n的有理數(shù)矩陣；

根據(jù)牛頓歐拉公式，帶有傾角的六旋翼無人機(jī)的動力學(xué)模型如下，

其中，六旋翼無人機(jī)的質(zhì)量為m，g為重力加速度，ω為六旋翼無人機(jī)的姿態(tài)角，為六旋翼無人機(jī)的角速度，J為轉(zhuǎn)動慣量，為加速度，e₃為地面坐標(biāo)系Z軸(0，0，1)。