[發(fā)明專利]基于全過程約束的四旋翼實時軌跡規(guī)劃及降落控制方法

有效

• 申請?zhí)枺?/td>: 202111575635.0
• 申請日: 2021-12-21
• 公開（公告）號: CN114594783B
• 公開（公告）日: 2023-03-31
• 發(fā)明（設計）人: 張福彪;王亞凱;林德福;莫靂;宋韜;周天澤;陳祺;劉明成;郎帥鵬;孫之問;王昭舜
• 申請（專利權）人: 北京理工大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 北京康思博達知識產(chǎn)權代理事務所(普通合伙) 11426
• 代理人: 劉冬梅

1.一種基于全過程約束的四旋翼實時軌跡規(guī)劃及降落控制方法，其特征在于，該方法包括如下步驟：

步驟一：獲得四旋翼飛行器與降落平臺的相對距離P_r和相對速度信息V_r；

步驟二：估計降落所需的時間，基于預測時域N獲得采樣時間長度；

步驟三：確定四旋翼飛行器當前的行為模式，規(guī)劃參考軌跡；

步驟四：通過控制器控制四旋翼飛行器跟蹤參考軌跡；

步驟五：到達第一個采樣點時，重復步驟一，并判斷四旋翼飛行器與降落平臺的相對距離P_r是否小于或等于預定閾值，若不小于或等于預定閾值，則重復步驟二、步驟三、步驟四、步驟五，直至四旋翼飛行器與降落平臺的相對距離P_r小于或等于預定閾值，四旋翼飛行器停機，落到降落平臺上；

所述參考軌跡包括N個狀態(tài)向量，每個狀態(tài)向量都包括坐標位置、飛行器到達該坐標位置時的速度和飛行器到達該坐標位置時的加速度；

所述規(guī)劃參考軌跡的過程包括，使得目標函數(shù)最小，

當行為模式為搜索模式時，所述目標函數(shù)為下式(2)，

當行為模式為著陸模式時，所述目標函數(shù)為下式(3)，

其中，J表示目標函數(shù)，i表示第i個采樣點，x_A(i)表示第i個采樣點的四旋翼飛行器與降落平臺的相對距離，表示x(i)的轉(zhuǎn)置；T_A表示降落所需的時間；Q_A和W_A都表示權重系數(shù)；

x_L表示四旋翼飛行器與降落平臺的相對距離P_r和相對速度V_r組成的矩陣，即x_L＝[P_r，V_r]^T；表示x_L的轉(zhuǎn)置；x_L(i)表示在第i個采樣點的四旋翼飛行器與降落平臺的相對距離P_r和相對速度V_r組成的矩陣；

u(i)表示第i個采樣點的控制量，u^T(i)表示u(i)的轉(zhuǎn)置；

a_L表示四旋翼飛行器水平面內(nèi)的加速度，即a_L＝[a_x，a_y]^T；

表示終端時刻N的加速度矢量；

Q_L、R_L、W_L都表示系數(shù)矩陣；

在所述規(guī)劃參考軌跡的過程中，基于下式(4.1)、(4.2)和(4.3)的約束使得目標函數(shù)最小，

x(k+1)＝Ax(k)+Bu(k) (4.1)

x(0)＝x₀，x(N)＝x_f，z(k+1)＞h_n (4.2)

|a_p(k+1)|≤a_p，max，|u_p(k)|≤u_p，max，p＝x，y，z (4.3)

其中，x(k+1)表示預測獲得的第k+1時刻的狀態(tài)向量，即第k+1個采樣點狀態(tài)向量；k＝0，1，...，N-1；

x(k)表示第k時刻的狀態(tài)向量，即第k個采樣點狀態(tài)向量；

A和B都表示系數(shù)矩陣；

u(k)表示第k個采樣點的控制量；

x(0)表示當前優(yōu)化時域中，初始時刻的無人機狀態(tài)，x₀表示當前時刻的無人機真實狀態(tài)；x(N)表示當前優(yōu)化時域中，終端時刻的無人機狀態(tài)x_f表示該優(yōu)化時域中，終端時刻無人機所需滿足的終端狀態(tài)約束；z(k+1)表示k+1時刻的無人機高度，h_p表示降落平臺的高度；

|a_p(k+1)|表示第k+1個采樣點的加速度，|u_p(k)|表示第k個采樣點的控制量；a_p，max表示飛行器能夠提供的最大加速度，u_p，max表示飛行器能夠提供的最大控制量。