[发明专利]基于全过程约束的四旋翼实时轨迹规划及降落控制方法

权利要求书

1.一种基于全过程约束的四旋翼实时轨迹规划及降落控制方法，其特征在于，该方法包括如下步骤：

步骤一：获得四旋翼飞行器与降落平台的相对距离P_r和相对速度信息V_r；

步骤二：估计降落所需的时间，基于预测时域N获得采样时间长度；

步骤三：确定四旋翼飞行器当前的行为模式，规划参考轨迹；

步骤四：通过控制器控制四旋翼飞行器跟踪参考轨迹；

步骤五：到达第一个采样点时，重复步骤一，并判断四旋翼飞行器与降落平台的相对距离P_r是否小于或等于预定阈值，若不小于或等于预定阈值，则重复步骤二、步骤三、步骤四、步骤五，直至四旋翼飞行器与降落平台的相对距离P_r小于或等于预定阈值，四旋翼飞行器停机，落到降落平台上；

所述参考轨迹包括N个状态向量，每个状态向量都包括坐标位置、飞行器到达该坐标位置时的速度和飞行器到达该坐标位置时的加速度；

所述规划参考轨迹的过程包括，使得目标函数最小，

当行为模式为搜索模式时，所述目标函数为下式(2)，

当行为模式为着陆模式时，所述目标函数为下式(3)，

其中，J表示目标函数，i表示第i个采样点，x_A(i)表示第i个采样点的四旋翼飞行器与降落平台的相对距离，表示x(i)的转置；T_A表示降落所需的时间；Q_A和W_A都表示权重系数；

x_L表示四旋翼飞行器与降落平台的相对距离P_r和相对速度V_r组成的矩阵，即x_L＝[P_r，V_r]^T；表示x_L的转置；x_L(i)表示在第i个采样点的四旋翼飞行器与降落平台的相对距离P_r和相对速度V_r组成的矩阵；

u(i)表示第i个采样点的控制量，u^T(i)表示u(i)的转置；

a_L表示四旋翼飞行器水平面内的加速度，即a_L＝[a_x，a_y]^T；

表示终端时刻N的加速度矢量；

Q_L、R_L、W_L都表示系数矩阵；

在所述规划参考轨迹的过程中，基于下式(4.1)、(4.2)和(4.3)的约束使得目标函数最小，

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时刻的状态向量，即第k+1个采样点状态向量；k＝0，1，...，N-1；

x(k)表示第k时刻的状态向量，即第k个采样点状态向量；

A和B都表示系数矩阵；

u(k)表示第k个采样点的控制量；

x(0)表示当前优化时域中，初始时刻的无人机状态，x₀表示当前时刻的无人机真实状态；x(N)表示当前优化时域中，终端时刻的无人机状态x_f表示该优化时域中，终端时刻无人机所需满足的终端状态约束；z(k+1)表示k+1时刻的无人机高度，h_p表示降落平台的高度；

|a_p(k+1)|表示第k+1个采样点的加速度，|u_p(k)|表示第k个采样点的控制量；a_p，max表示飞行器能够提供的最大加速度，u_p，max表示飞行器能够提供的最大控制量。