CN113961020A - 一种无人机三维空间运动控制方法及系统 - Google Patents

Abstract

本发明实施例涉及无人机技术领域，具体提供了一种无人机三维空间运动控制方法及系统，方法包括：基于第一算法计算得到无人机在三维空间运动时的三维目标合速度

，基于第二算法将三维目标合速度

分解为水平目标速度

和竖直目标速度

，根据所述水平目标速度

和竖直目标速度

分别对应在水平方向、竖直方向做串级PID控制，以得到无人机驱动电机的控制量，并根据无人机驱动电机的控制量控制无人机向运动目标点A运动。本发明实施例能够避免因水平方向、竖直方向距离悬殊过大而导致的距离较短的方向先到达目标点、而距离较长的方向后达到目标点的情形出现，同时避免发生无人机撞击到障碍物的事故发生，以对无人机实现精准的运动控制。

Description

一种无人机三维空间运动控制方法及系统

技术领域

本发明实施例涉及无人机技术领域，特别涉及一种无人机三维空间运动控制方法及系统。

背景技术

无人机自动巡检功能是将无人机作为移动载体运用到工程实际的重要方式，其已经在电网、森林、植保等具体领域得以广泛应用。而无人机自动巡检功能主要是依托无人机航迹规划方式来实现无人机在三维空间的运动控制。

现有的无人机自动巡检飞行控制中，当无人机需要从当前点（O点）运动到目标点（A点）时，先以O点作为坐标原点建立三维坐标系OXYZ（其中XY为水平面，OZ为竖直方向），然后将向量OA分别投影到水平方向和竖直方向，分别获取向量OA的水平分量S和竖直分量H，在得到无人机水平目标量S和竖直目标量H后，基于常规PID闭环控制算法，两个方向控制量会作为控制器的期望给入，然后经过控制器算法生成电机驱动量，根据电机驱动量控制无人机向目标点飞行。由于无人机水平方向和竖直方向控制是相互独立的，因此，在无人机飞向A点的过程中，水平方向和竖直方向的运动可能会出现偏差，导致无人机在水平方向、竖直方向上不能同时到达A点，导致无法达到预期自动巡检目的。

发明内容

本发明实施例的目的在于提供一种无人机三维空间运动控制方法及系统，以解决目前无人机飞向目标点的过程中，水平方向和竖直方向的运动可能会出现偏差，导致无人机在水平方向、竖直方向上不能同时到达目标点，导致无法达到预期自动巡检目的的问题。为实现上述目的，本发明实施例提供了如下的技术方案。

第一方面，在本发明提供的一个优选实施方式中，提供了一种无人机三维空间运动控制方法，所述的控制方法包括以下步骤：

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

，

基于第二算法将所述三维目标合速度

分解为水平目标速度

和竖直目标 速度

，

根据所述水平目标速度

和竖直目标速度

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

在本发明提供的一些实施例中，所述第一算法所采用公式如下：

；

其中，

分别为PID控制算法的比例、积分、微分系数。

在本发明提供的一些实施例中，所述第二算法所采用的的公式如下：

；

其中，m1为运动距离m在水平方向的投影长度，

为运动距离m在竖直方向的投 影长度。

在本发明提供的一些实施例中，在所述无人机运动前：所述控制方法还包括以下步骤：

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

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

在本发明提供的一些实施例中，在控制无人机开始从O点向A点运动时，所述运动 过程中，获取无人机的实时位置C点，且C点到O点之间的距离记为实时距离n，且n

m，所述 无人机的实时位置C点到A点之间的实时距离d=m-n。

在本发明提供的一些实施例中，所述获取无人机的实时位置C点的步骤具体包括：通过安装在所述无人机上的GPS模块传感器测量得到所述无人机的实时位置C点。

第二方面，在本发明的另一个实施方式中，提供了一种无人机三维空间运动控制系统，所述的控制系统包括：

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

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

，

第二计算单元，用于基于第二算法将所述三维目标合速度

分解为水平目标速 度

和竖直目标速度

，

飞行控制单元，用于根据所述水平目标速度

和竖直目标速度

分别对应在 水平方向、竖直方向做串级PID控制，以得到无人机驱动电机的控制量；

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

在本发明提供的一些实施例中，所述的控制系统还包括：

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

在本发明提供的一些实施例中，所述的控制系统还包括：

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

m，所述无人机的实时位置C点到A 点之间的实时距离d=m-n。

在本发明提供的一些实施例中，所实时距离确定单元包括：

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

本发明实施例提供的无人机三维空间运动控制方向及系统的技术效果：

先通过无人机运动的预定距离、PID位置环控制器计算得到目标合速度，然后在将目标合速度进行几何分解得到水平方向、竖直方向两个目标分速度，由此可保证无人机从实时位置向目标点的三维空间运动过程中，在水平方向和竖直方向能同时到达目标点，从而避免因水平方向、竖直方向距离悬殊过大而导致的距离较短的方向先到达目标点、而距离较长的方向后达到目标点的情形出现，同时避免发生无人机撞击到障碍物的事故发生，以对无人机实现更为精准的运动控制。

附图说明

图1是本发明一种无人机三维空间运动控制方法的实现流程图；

图2是本发明一种无人机三维空间运动控制系统的结构框图；

图3是本发明一种无人机三维空间运动控制方法的应用示意图。

具体实施方式

下面将结合本发明的附图，对本发明的技术方案进行清楚、完整地描述，显然，所举实例用于解释说明，并非限定本发明的实施方式，本发明也可以通过其它不同的具体实施方式实施。本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

现有的无人机自动巡检飞行控制中，当无人机需要从当前点（O点）运动到目标点（A点）时，先以O点作为坐标原点建立三维坐标系OXYZ（其中XY为水平面，OZ为竖直方向），然后将向量OA分别投影到水平方向和竖直方向，分别获取向量OA的水平分量S和竖直分量H，在得到无人机水平目标量S和竖直目标量H后，基于常规PID闭环控制算法，两个方向控制量会作为控制器的期望给入，然后经过控制器算法生成电机驱动量，根据电机驱动量控制无人机向目标点飞行。由于无人机水平方向和竖直方向控制是相互独立的，因此，在无人机飞向A点的过程中，水平方向和竖直方向的运动可能会出现偏差，导致无人机在水平方向、竖直方向上不能同时到达A点，导致无法达到预期自动巡检目的。

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地描述。

图1示例性的示出了本发明实施例提供的一种无人机三维空间运动控制方法的实现流程图。

如图1所示，在本发明提供的一个优选实施方式中，提供了一种无人机三维空间运动控制方法，所述的控制方法S100包括以下步骤：

步骤S101：在无人机运动过程中，将无人机的实时位置C点到运动目标点A点之间的距离d输入PID位置环控制器；

步骤S102：基于第一算法计算得到所述无人机在三维空间运动时的三维目标合速 度

，

步骤S103：基于第二算法将所述三维目标合速度

分解为水平目标速度

和 竖直目标速度

，

步骤S104：根据所述水平目标速度

和竖直目标速度

分别对应在水平方 向、竖直方向做串级PID控制，以得到无人机驱动电机的控制量；

步骤S105：根据所述无人机驱动电机的控制量控制无人机向运动目标点A运动。

进一步的，在本发明提供的优选实施方式中，在所述无人机运动前：所述控制方法还包括以下步骤：

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

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

进一步的，在本发明提供的优选实施方式中，在控制无人机开始从O点向A点运动 时，所述运动过程中，获取无人机的实时位置C点，且C点到O点之间的距离记为实时距离n， 且n

m，所述无人机的实时位置C点到A点之间的实时距离d=m-n。

进一步的，在本发明提供的优选实施方式中，所述获取无人机的实时位置C点的步骤具体包括：通过安装在所述无人机上的GPS模块传感器测量得到所述无人机的实时位置C点。

进一步的，在本发明提供的优选实施方式中，所述第一算法所采用公式如下：

；

其中，

分别为PID控制算法的比例、积分、微分系数，其中，所述比例、 所述积分、所述微分是PID控制算法里的专业术语。

的值是通过对具体的无人 机调试后得到，具体不进行限定。

进一步的，在本发明提供的优选实施方式中，所述第二算法所采用的的公式如下：

；

其中，m1为运动距离m在水平方向的投影长度，

为运动距离m在竖直方向的投 影长度。

具体的，在无人机的应用中，例如自动巡检过程中，在无人机实时位点C正前方有障碍物时，把障碍物顶底向上一段距离作为无人机的目标点A，此时无人机实时位点C和目标点A三维空间位置在水平方向过小于竖直方向，具体的，如图3所示状态，目标点A位于障碍物正上方，现有的控制算法是先把OA分解得到水平目标距离S和竖直目标距离H，然后两个分方向进行位置控制使无人机向目标点A飞行。由于两个分方向是各自独立控制，若水平方向的控制使无人机到达水平目标距离，但竖直方向的控制使无人机的爬升高度还没超过障碍物高度，则此时无人机必然撞上障碍物。

若按照现有的控制方式，将实时位点C到A点的向量分别投影到水平方向和竖直方向，以此对无人机在水平方向和竖直独立的运动进行独立控制，则无人机会撞上障碍物而无法到达目标点；而本实施例中的控制方法可先通过无人机运动的预定距离、PID位置环控制器计算得到目标合速度，然后在将目标合速度进行几何分解得到水平方向、竖直方向两个目标分速度，由此可保证无人机从实时位置向目标点的三维空间运动过程中，在水平方向和竖直方向能同时到达目标点，从而避免因水平方向、竖直方向距离悬殊过大而导致的距离较短的方向先到达目标点、而距离较长的方向后达到目标点的情形出现，同时避免发生无人机撞击到障碍物的事故发生，以对无人机实现更为精准的运动控制。

具体的，本发明的位置控制算法是先把三维目标距离m进行位置控制，得到三维合 目标速度

，然后在把三维合速度vt0基于几何关系进行分解得到水平方向目标速度

和竖直目标速度

，然后再两个方向上继续进行串级PID控制，最终控制无人机向目 标点飞行。本设计算法水平和竖直两个方向是相互关联而不是各自独立的，而本算计基于 的数学关系的控制效果是水平方向和竖直方向同时到达，即若水平方向到达了水平目标距 离，那竖直方向也必然到达了竖直目标距离，故在飞向目标点的过程中不会撞到障碍物。

图2示例性的示出了本发明实施例提供的种无人机三维空间运动控制系统的结构框图。

如图2所示，在本发明的另一个实施方式中，提供了一种无人机三维空间运动控制系统，所述的控制系统200包括：

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

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

，

第二计算单元203，用于基于第二算法将所述三维目标合速度

分解为水平目 标速度

和竖直目标速度

，

飞行控制单元204，用于根据所述水平目标速度

和竖直目标速度

分别对 应在水平方向、竖直方向做串级PID控制，以得到无人机驱动电机的控制量；

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

进一步的，在本发明提供的优选实施方式中，所述的控制系统还包括：

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

进一步的，在本发明提供的优选实施方式中，所述的控制系统还包括：

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

m，所述无人机的实时位置C点到A 点之间的实时距离d=m-n。

进一步的，在本发明提供的优选实施方式中，所实时距离确定单元包括：

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

本发明实施例提供的无人机三维空间运动控制方向及系统的技术效果：本发明实施例先通过无人机运动的预定距离、PID位置环控制器计算得到目标合速度，然后在将目标合速度进行几何分解得到水平方向、竖直方向两个目标分速度，由此可保证无人机从实时位置向目标点的三维空间运动过程中，在水平方向和竖直方向能同时到达目标点，从而避免因水平方向、竖直方向距离悬殊过大而导致的距离较短的方向先到达目标点、而距离较长的方向后达到目标点的情形出现，同时避免发生无人机撞击到障碍物的事故发生，以对无人机实现更为精准的运动控制。

本发明实施例还提供了一种电子设备，该电子设备可以是手机、电脑或平板电脑等，包括存储器和处理器，所述存储器上存储有计算机程序，该计算机程序被处理器执行时实现如上述实施例提供的所述的无人机三维空间运动控制方法。可以理解，电子设备还可以包括，输入/输出(I/O)接口，以及通信组件。

其中，处理器用于执行如上述实施例的无人机三维空间运动控制方法中的全部或部分步骤。存储器用于存储各种类型的数据，这些数据例如可以包括电子设备中的任何应用程序或方法的指令，以及应用程序相关的数据。

所述处理器可以是专用集成电路(Application Specific Integrated Cricuit，简称ASIC)、数字信号处理器(Digital Signal Processor，简称DSP)、可编程逻辑器件(Programmable Logic Device，简称PLD)、现场可编程门阵列(Field Programmable GateArray，简称FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述实施例提供的所述的无人机三维空间运动控制方法。

所述存储器可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，例如静态随机存取存储器(Static Random Access Memery，简称SRAM)，电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory，简称EEPROM) ,可擦除可编程只读存储器(Erasable Programmable Read-Only Memory，简称EPROM) ,可编程只读存储器(Programmable Read-Only Memory，简称PROM) ,只读存储器(Read-OnlyMemory，简称ROM)，磁存储器，快闪存储器，磁盘或光盘。

本实施例还提供一种计算机可读存储介质。在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。

基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。

而前述的存储介质包括：闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等)、随机访问存储器(RAM)、静态随机访问存储器(SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘、服务器、APP应用商城等等各种可以存储程序校验码的介质，其上存储有计算机程序，所述计算机程序被处理器执行时可以实现如下方法步骤：

步骤S101：在无人机运动过程中，将无人机的实时位置C点到运动目标点A点之间的距离d输入PID位置环控制器；

步骤S102：基于第一算法计算得到所述无人机在三维空间运动时的三维目标合速 度

，

步骤S103：基于第二算法将所述三维目标合速度

分解为水平目标速度

和 竖直目标速度

，

步骤S104：根据所述水平目标速度

和竖直目标速度

分别对应在水平方 向、竖直方向做串级PID控制，以得到无人机驱动电机的控制量；

步骤S105：根据所述无人机驱动电机的控制量控制无人机向运动目标点A运动。

上述方法步骤的具体实施例过程可参见实施例一，本实施例在此不再重复赘述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。本申请实施例中所有方向性指示(诸如上、下、左、右、前、后、顶、底……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

另外，在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换等，均应包含在本发明的保护范围之内。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换等，均应包含在本发明的保护范围之内。

Claims (9)

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

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

，其中，所述第一算法所采用公式如下：

；

其中，

分别为PID控制算法的比例、积分、微分系数；

基于第二算法将所述三维目标合速度

分解为水平目标速度

和竖直目标速度

；

根据所述水平目标速度

和竖直目标速度

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

2.根据权利要求1所述的无人机三维空间运动控制方法，其特征在于，在所述无人机运动前，所述控制方法还包括以下步骤：

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

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

3.根据权利要求2所述的无人机三维空间运动控制方法，其特征在于，在控制无人机开 始从O点向A点运动时，所述运动过程中，获取无人机的实时位置C点，且C点到O点之间的距 离记为实时距离n，且n

m，所述无人机的实时位置C点到A点之间的实时距离d=m-n。

4.根据权利要求3所述的无人机三维空间运动控制方法，其特征在于，所述获取无人机的实时位置C点的步骤具体包括：通过安装在所述无人机上的GPS模块传感器测量得到所述无人机的实时位置C点。

5.根据权利要求3或4所述的无人机三维空间运动控制方法，其特征在于，所述第二算法所采用的公式如下：

；

其中，m1为运动距离m在水平方向的投影长度，

为运动距离m在竖直方向的投影长度。

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

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

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

，

第二计算单元，用于基于第二算法将所述三维目标合速度

分解为水平目标速度

和竖直目标速度

，

飞行控制单元，用于根据所述水平目标速度

和竖直目标速度

分别对应在水平 方向、竖直方向做串级PID控制，以得到无人机驱动电机的控制量；

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

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

8.根据权利要求7所述的无人机三维空间运动控制系统，其特征在于，所述的控制系统还包括：

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

m，所述无人机的实时位置C点到A点之 间的实时距离d=m-n。

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

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

Images