[發明專利]一種考慮姿態約束的高機動微型無人機控制方法

有效

• 申請號: 202110065566.2
• 申請日: 2021-01-14
• 公開（公告）號: CN112684805B
• 公開（公告）日: 2021-11-23
• 發明（設計）人: 蔡志浩;孫幫樂;趙江;王英勛;孫家嶺
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G05D1/08
• 分類號: G05D1/08;G05D1/10
• 代理公司: 北京航智知識產權代理事務所(普通合伙) 11668
• 代理人: 黃川;史繼穎

1.一種考慮姿態約束的高機動微型無人機控制方法，其特征在于，包括以下步驟：

S1：搭建高機動微型無人機的非線性運動學和動力學模型，采用旋轉矩陣表示高機動微型無人機的姿態；其中，高機動微型無人機的運動學模型為：

其中，X₁＝[P R]^T，X₂＝[v ω]^T分別為運動學模型和動力學模型的狀態向量；為運動學模型的狀態向量的導數，其中，P＝[x，y，z]為高機動微型無人機在地面坐標系下的位置坐標，R＝[r₁，r₂，r₃]為滾轉角、俯仰角和偏航角組成的旋轉矩陣；v＝[v_x，v_y，v_z]為地面坐標系下的速度矢量，ω＝[p，q，r]為機體坐標系下角速度矢量，p為高機動微型無人機滾轉角速度，q為高機動微型無人機俯仰角速度，r為高機動微型無人機偏航角速度；P(X₁)是非線性函數；

高機動微型無人機的動力學方模型為：

其中，為動力學模型的狀態向量的導數，U＝[T，τ_x，τ_y，τ_z]^T為控制輸入向量，T為高機動微型無人機拉力輸入，τ_x，τ_y，τ_z分別為高機動微型無人機在各機體軸上的滾轉力矩輸入、俯仰力矩輸入和偏航力矩輸入，F(X₁，X₂)是非線性函數，g是非線性控制分配函數；

S2：設計位置控制算法；根據步驟S1中的非線性運動學和動力學模型，將控制算法設計問題轉為位置控制設計和姿態控制設計兩個子問題，設計位置控制算法得出所需的加速度控制指令以及期望拉力控制輸入；具體地，依據平移運動設計位置控制算法，平移運動模型為：

其中，g為加速度向量，R為旋轉矩陣，z_w為慣性坐標系中z軸坐標軸向量，m為高機動微型無人機質量；

由平移運動模型知，平移運動包含位置P信息、速度v信息、產生期望加速度指令a_d和期望拉力T_d，則狀態變量和期望輸入指令為：

x₁＝[P v]^T

u₁＝[a_d T_d]^T

其中，x₁為涉及平移運動的狀態變量，u₁為平移運動的控制輸入；

控制目的是設計期望加速度a_d，使以及成立；將位置誤差e_p(t)簡化為e_p，將速度誤差e_v(t)簡化為e_v，則位置誤差e_P＝P_d-P，速度誤差e_v＝v_d-v，P_d，v_d分別為期望位置和期望速度；

整個位置控制算法采用串級PID進行設計，位置內回路為速度環，采用P控制，保證速度控制快速響應，由平移動力學特性加入非線性補償項，記K_v，P為內回路比例控制算法參數，具體算法為：

a_d＝K_v，Pe_v-g

外回路同樣采用P控制，記v′為虛擬控制輸入，記K_P，P為外回路比例控制算法參數，具體算法為：

v′＝K_P，Pe_p

最后，記機體坐標系中的z坐標軸向量在慣性坐標系的投影向量為由此計算出期望拉力：

S3：解算期望旋轉矩陣；由步驟S2的位置控制算法輸出的期望加速度解算出所需的旋轉矩陣以實現姿態控制；

S4：設計姿態控制算法；根據步驟S3解算的期望旋轉矩陣設計姿態誤差矩陣，依據非線性動力學對姿態控制算法進行補償，計算出所需期望力矩，完成姿態控制算法設計；

S5：調節控制算法參數；所述算法是針對姿態約束的場景，固定的控制增益不能控制所有高機動微型無人機飛行狀態，需在飛行時利用增益調度的方法調節控制算法參數，保證多種飛行狀態乃至姿態約束時高機動微型無人機的可控性。