[发明专利]一种四旋翼无人机控制方法、装置、终端设备及存储介质

权利要求书

1.一种四旋翼无人机控制方法，其特征在于，包括：

根据第k时刻的十二维控制状态量和第k时刻的四维控制输入量，获取第k+n时刻的十二维控制状态量；

根据所述第k+n时刻的十二维控制状态量和所述第k+n时刻的十二维控制状态量的期望值，构建第k+n时刻的目标函数；

基于非线性模型预测控制方法，对所述第k+n时刻的目标函数进行极小值求解，获取满足约束条件的第k+1时刻的四维控制输入量；

根据第k+1时刻的四维控制输入量，对四旋翼无人机在第k+1时刻的六维位姿数据进行控制；

其中，所述十二维控制状态量包括六维位姿数据和所述六维位姿数据的一阶微分，所述六维位姿数据包括地球坐标系下的三维位置、滚转角、俯仰角和偏航角，所述四维控制输入量包括机体坐标系下的合升力、滚转力矩、俯仰力矩和偏航力矩，所述约束条件基于所述四维控制输入量构建，k和n为正整数。

2.如权利要求1所述的四旋翼无人机控制方法，其特征在于，所述根据第k时刻的十二维控制状态量和第k时刻的四维控制输入量，获取第k+n时刻的十二维控制状态量，包括：

根据第k+i-1时刻的十二维控制状态量和第k+i-1时刻的四维控制输入量，获取第k+i时刻的六维位姿数据的二阶微分；

根据第k+i-1时刻的六维位姿数据的一阶微分、第k+i-1时刻的六维位姿数据的二阶微分以及第k+i时刻与第k+i-1时刻之间的间隔时间，获取第k+i时刻的六维位姿数据的一阶微分；

根据第k+i-1时刻的十二维控制状态量、第k+i-1时刻的六维位姿数据的二阶微分以及第k+i时刻与第k+i-1时刻之间的间隔时间，获取第k+i时刻的六维位姿数据；

其中，i＝1,2,…,n。

3.如权利要求2所述的四旋翼无人机控制方法，其特征在于，所述第k+i时刻的六维位姿数据的二阶微分的计算公式为：

所述第k+i时刻的六维位姿数据的一阶微分的计算公式为：

所述第k+i时刻的六维位姿数据的计算公式为：

其中，表示所述第k+i时刻的六维位姿数据的二阶微分，表示所述第k+i时刻的六维位姿数据的一阶微分，X(k+i)表示所述第k+i时刻的六维位姿数据，f()表示四旋翼无人机的动力学方程的函数，X(k+i-1)表示所述第k+i-1时刻的六维位姿数据，表示所述第k+i-1时刻的六维位姿数据的一阶微分，U(k+i-1)表示所述第k+i-1时刻的四维控制输入量，表示所述第k+i-1时刻的六维位姿数据的二阶微分，T_k+i表示第k+i时刻对应的时间点，T_k+i-1表示第k+i-1时刻对应的时间点。

4.如权利要求3所述的四旋翼无人机控制方法，其特征在于，所述四旋翼无人机的动力学方程的表达式为：

其中，表示所述六维位姿数据，表示所述六维位姿数据的一阶微分，表示所述六维位姿数据的二阶微分，[I_x I_y I_z]表示所述地球坐标系下的三维旋转惯量，[U₁ U₂ U₃ U₄]^T表示所述四维控制输入量，m表示所述四旋翼无人机的重量，g表示重力加速度。

5.如权利要求1至4任一项所述的四旋翼无人机控制方法，其特征在于，所述第k+n时刻的目标函数的表达式为：

minW²

W＝Y_d(k+n)-Y(k+n)

其中，minW²表示所述第k+n时刻的目标函数，min()表示求最小值函数，W表示所述第k+n时刻的十二维控制状态量的期望值与所述第k+n时刻的十二维控制状态量之间的误差，Y_d(k+n)表示所述第k+n时刻的十二维控制状态量的期望值，Y(k+n)表示所述第k+n时刻的十二维控制状态量。