[发明专利]一种无人机三维空间运动控制方法及系统

权利要求书

1.一种无人机三维空间运动控制方法，其特征在于，所述的控制方法包括以下步骤：

在无人机运动过程中，将无人机的实时位置C点到运动目标点A点之间的距离d输入PID位置环控制器，并基于第一算法计算得到所述无人机在三维空间运动时的三维目标合速度，其中，所述第一算法所采用公式如下：

；

其中，k_p、k_i、k_d分别为PID控制算法的比例、积分、微分系数；

基于第二算法将所述三维目标合速度分解为水平目标速度和竖直目标速度；

根据所述水平目标速度和竖直目标速度分别对应在水平方向、竖直方向做串级PID控制，以得到无人机驱动电机的控制量，并根据所述无人机驱动电机的控制量控制无人机向运动目标点A运动；

在所述无人机运动前，所述控制方法还包括以下步骤：

获取无人机运动的起始位点O点以及运动目标点A点；

以O点作为坐标原点建立三维坐标系OXYZ，其中，XY为水平面，OZ为竖直方向；O点到A点的距离记为预定运动距离m，m≠0；

在控制无人机开始从O点向A点运动时，所述运动过程中，获取无人机的实时位置C点，且C点到O点之间的距离记为实时距离n，且nm，所述无人机的实时位置C点到A点之间的实时距离d=m-n；

所述获取无人机的实时位置C点的步骤具体包括：通过安装在所述无人机上的GPS模块传感器测量得到所述无人机的实时位置C点；

所述第二算法所采用的公式如下：

；

其中，m₁为运动距离m在水平方向的投影长度，m₂为运动距离m在竖直方向的投影长度。

2.一种无人机三维空间运动控制系统，其特征在于，所述的控制系统包括：

数据输入单元，在无人机运动过程中，将无人机的实时位置C点到运动目标点A点之间的距离d输入PID位置环控制器，

第一计算单元，用于基于第一算法计算得到所述无人机在三维空间运动时的三维目标合速度，

第二计算单元，用于基于第二算法将所述三维目标合速度分解为水平目标速度和竖直目标速度，

飞行控制单元，用于根据所述水平目标速度和竖直目标速度分别对应在水平方向、竖直方向做串级PID控制，以得到无人机驱动电机的控制量；

飞行执行单元，用于根据所述无人机驱动电机的控制量控制无人机向运动目标点A运动；

所述的控制系统还包括：运动距离确定单元，用于获取无人机运动的起始位点O点以及运动目标点A点；以O点作为坐标原点建立三维坐标系OXYZ，其中，XY为水平面，OZ为竖直方向；O点到A点的距离记为预定运动距离m，m≠0；

所述的控制系统还包括：

实时距离确定单元，用于在控制无人机开始从O点向A点运动时，获取无人机的实时位置C点，且C点到O点之间的距离记为实时距离n，且nm，所述无人机的实时位置C点到A点之间的实时距离d=m-n。

3.根据权利要求2所述的无人机三维空间运动控制系统，其特征在于，所实时距离确定单元包括：

GPS模块传感器，用于测量获取所述无人机的实时位置C点。