[發明專利]一種六旋翼無人機動力缺失保護系統、保護方法

有效

• 申請號: 202011295521.6
• 申請日: 2020-11-18
• 公開（公告）號: CN112373677B
• 公開（公告）日: 2021-06-22
• 發明（設計）人: 劉鵬;謝雨;趙坤;劉蘇;劉帥
• 申請（專利權）人: 三生萬物（北京）人工智能技術有限公司
• 主分類號: B64C19/00
• 分類號: B64C19/00;B64D31/00;B64D45/00
• 代理公司: 北京權智天下知識產權代理事務所(普通合伙) 11638
• 代理人: 王新愛

1.一種六旋翼無人機動力缺失保護方法，其特征在于，包括以下步驟：

S1：設置飛機的最佳油門為每個電機的中間油門：

best_thr_i＝(thr_min_i+thr_max_i)/2

thr_min_i表示第i號漿葉電機的最小油門值；

thr_max_i表示第i號漿葉電機的最大油門值；

best_thr_i表示第i號漿葉電機的最佳油門值；

其中，i＝0、1、2、3、4、5；

S2：

按照下述分配原則，分別把橫滾油門、俯仰油門分配到每一個電機:

thr_0＝best_thr_0+picth_thr×cos(-30°)+roll_thr×sin(-30°)

thr_1＝best_thr_1+pitch_thr×cos30°+roll_thr×sin30°

thr_2＝best_thr_2+pitch_thr×cos90°+roll_thr×sin90°

thr_3＝best_thr_3+pitch_thr×cos150°+roll_thr×sin150°

thr_4＝best_thr_4+pitch_thr×cos(-150°)+roll_thr×sin(-150°)

thr_5＝best_thr_5+pitch_thr×cos(-90°)+roll_thr×sin(-90°)

thr_0表示：第0號漿葉電機的油門輸出值；

thr_1表示：第1號漿葉電機的油門輸出值；

thr_2表示：第2號漿葉電機的油門輸出值；

thr_3表示：第3號漿葉電機的油門輸出值；

thr_4表示：第4號漿葉電機的油門輸出值；

thr_5表示：第5號漿葉電機的油門輸出值；

其中，Pitch_thr表示：飛控系統計算出來用于保持無人機俯仰動作的油門量；Roll_thr表示：飛控系統計算出來用于保持無人機橫滾動作的油門量；即Pitch_thr、Roll_thr從飛控系統查詢得到；

S3：

S3-1,求取所有電機中的油門最大值motor_thr_max和最小值motor_thr_min，即：

所有電機中的油門最大值motor_thr_max為thr_0、thr1、thr2、thr3、thr4、thr5的最大值；

所有電機中的油門最小值motor_thr_min為thr_0、thr1、thr2、thr3、thr4、thr5的最小值；

S3-2,計算還剩下的油門空間：

首先，計算：thr_max、thr_min；

thr_max是thr_max_0、thr_max_1、thr_max_2、thr_max_3、thr_max_4、thr_max_5的最大值；

thr_min是thr_min_0、thr_min_1、thr_min_2、thr_min_3、thr_min_4、thr_min_5的最小值；

然后，然后計算參數：thr_adj_max、thr_adj_min；

thr_adj_max＝thr_max-motor_thr_max；

thr_adj_min＝thr_min-motor_thr_min；

thr_adj_max表示為了不使每個電機達到上限還剩余的油門量；

thr_adj_min表示為了不使每個電機達到下限還剩的油門量；

S4：

比較油門量與剩下的油門空間，并確定還剩的油門區間：

當飛機在升高時，即alt_thr大于0時；

alt_adj等于min(alt_thr,thr_adj_max)，即取為alt_thr、thr_adj_max的較小值；

當飛機在降低時，即alt_thr小于0時；

alt_adj＝max(alt_thr,thr_adj_min)，即取為alt_thr,thr_adj_min的較大值；

其中，alt_thr表示是飛控系統計算得到保證飛機做高度方向動作的油門，其從飛控系統查詢得到；

其中，alt_adj表示油門適應值參數；

S5：

S5-1,計算航向油門剩余量yaw_allowed，并分配航向油門：

yaw_allowed＝min(min(thr_adj_max,-thr_adj_min)-alt_adj,yaw_thr)

即“thr_adj_max,-thr_adj_min的較小值減去-alt_adj的值”與“yaw_thr”的較小值賦值給yaw_allowed；

yaw_allowed表示分配完橫滾油門、俯仰油門、高度油門后，能用于控制飛機機頭方向(注意不是航線方向)的油門剩余量，其采用上式計算得到；

yaw_thr表示航向油門，該數值由 飛控系統計算得到；

S5-2,

限制航向油門分配的最小值為100，即防止無人機旋轉太快，在落地時候損壞設備：

若S5-1中的yaw_allowed計算結果小于100，則yaw_allowed取值為100；

若S5-1中的yaw_allowed計算結果大于100，則yaw_allowed不變；

S6：

S6-1,將alt_adj與yaw_allowed分別加到S2得到的每個電機上，即：

thr_motor_i＝thr_i+alt_adj+yaw_allowed；

其中，i＝0、1、2、3、4、5；

其中，thr_motor_i表示第i號漿葉電機的油門；

S6-2,計算6個電機油門中的最大值last_thr_max，最小值last_thr_min

last_thr_max為thr_motor_0、thr_motor_1、thr_motor_2、thr_motor_3、thr_motor_4、thr_motor_5的最大值；

last_thr_min表示thr_motor_0、thr_motor_1、thr_motor_2、thr_motor_3、thr_motor_4、thr_motor_5的最小值；

S7：判斷每個電機是否在正常油門范圍內：

即thr_motor_i是否在：區間[thr_min_i，thr_max_i]之間：

當thr_motor_i在區間[thr_min_i，thr_max_i]時，thr_motor_i不變，轉入步驟S8；

當thr_motor_i大于thr_max_i，或者last_thr_min小于thr_min_i時，即如果有電機超過限幅；則進行以下操作：

調整次數為奇數次時，alt_adj采用原先計算的90％，即降低10％；alt_adj調整后的結果代入步驟S6中，重新計算；

調整次數為偶數次時，thr_i采用原先計算的90％，即降低10％，thr_i調整后的結果代入步驟S6中，重新計算；

S8：

將每個電機油門thr_motor_i發送到每個電機：完成各個動作優先級的分配。