一种无人机三维空间运动控制方法及系统转让专利
申请号 : CN202111580932.4
文献号 : CN113961020B
文献日 : 2022-04-08
发明人 : 黄立 , 张钦彦 , 林家民 , 张原艺 , 王龙 , 薛源 , 刘华斌
申请人 : 普宙科技(深圳)有限公司
摘要 :
权利要求 :
1.一种无人机三维空间运动控制方法,其特征在于,所述的控制方法包括以下步骤:在无人机运动过程中,将无人机的实时位置C点到运动目标点A点之间的距离d输入PID位置环控制器,并基于第一算法计算得到所述无人机在三维空间运动时的三维目标合速度,其中,所述第一算法所采用公式如下:;
其中,kp、ki、kd分别为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;
所述获取无人机的实时位置C点的步骤具体包括:通过安装在所述无人机上的GPS模块传感器测量得到所述无人机的实时位置C点;
所述第二算法所采用的公式如下:;
其中,m1为运动距离m在水平方向的投影长度,m2为运动距离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,且n m,所述无人机的实时位置C点到A点之间的实时距离d=m‑n。
3.根据权利要求2所述的无人机三维空间运动控制系统,其特征在于,所实时距离确定单元包括:
GPS模块传感器,用于测量获取所述无人机的实时位置C点。
说明书 :
一种无人机三维空间运动控制方法及系统
技术领域
背景技术
航迹规划方式来实现无人机在三维空间的运动控制。
后将向量OA分别投影到水平方向和竖直方向,分别获取向量OA的水平分量S和竖直分量H,
在得到无人机水平目标量S和竖直目标量H后,基于常规PID闭环控制算法,两个方向控制量
会作为控制器的期望给入,然后经过控制器算法生成电机驱动量,根据电机驱动量控制无
人机向目标点飞行。由于无人机水平方向和竖直方向控制是相互独立的,因此,在无人机飞
向A点的过程中,水平方向和竖直方向的运动可能会出现偏差,导致无人机在水平方向、竖
直方向上不能同时到达A点,导致无法达到预期自动巡检目的。
发明内容
机在水平方向、竖直方向上不能同时到达目标点,导致无法达到预期自动巡检目的的问题。
为实现上述目的,本发明实施例提供了如下的技术方案。
合速度 ,
无人机向运动目标点A运动。
无人机的实时位置C点到A点之间的实时距离d=m‑n。
记为预定运动距离m,所述m≠0。
点之间的实时距离d=m‑n。
实时位置向目标点的三维空间运动过程中,在水平方向和竖直方向能同时到达目标点,从
而避免因水平方向、竖直方向距离悬殊过大而导致的距离较短的方向先到达目标点、而距
离较长的方向后达到目标点的情形出现,同时避免发生无人机撞击到障碍物的事故发生,
以对无人机实现更为精准的运动控制。
附图说明
具体实施方式
施方式实施。本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施
例,都属于本发明保护的范围。
后将向量OA分别投影到水平方向和竖直方向,分别获取向量OA的水平分量S和竖直分量H,
在得到无人机水平目标量S和竖直目标量H后,基于常规PID闭环控制算法,两个方向控制量
会作为控制器的期望给入,然后经过控制器算法生成电机驱动量,根据电机驱动量控制无
人机向目标点飞行。由于无人机水平方向和竖直方向控制是相互独立的,因此,在无人机飞
向A点的过程中,水平方向和竖直方向的运动可能会出现偏差,导致无人机在水平方向、竖
直方向上不能同时到达A点,导致无法达到预期自动巡检目的。
且n m,所述无人机的实时位置C点到A点之间的实时距离d=m‑n。
点。
试后得到,具体不进行限定。
标点A三维空间位置在水平方向过小于竖直方向,具体的,如图3所示状态,目标点A位于障
碍物正上方,现有的控制算法是先把OA分解得到水平目标距离S和竖直目标距离H,然后两
个分方向进行位置控制使无人机向目标点A飞行。由于两个分方向是各自独立控制,若水平
方向的控制使无人机到达水平目标距离,但竖直方向的控制使无人机的爬升高度还没超过
障碍物高度,则此时无人机必然撞上障碍物。
无法到达目标点;而本实施例中的控制方法可先通过无人机运动的预定距离、PID位置环控
制器计算得到目标合速度,然后在将目标合速度进行几何分解得到水平方向、竖直方向两
个目标分速度,由此可保证无人机从实时位置向目标点的三维空间运动过程中,在水平方
向和竖直方向能同时到达目标点,从而避免因水平方向、竖直方向距离悬殊过大而导致的
距离较短的方向先到达目标点、而距离较长的方向后达到目标点的情形出现,同时避免发
生无人机撞击到障碍物的事故发生,以对无人机实现更为精准的运动控制。
和竖直目标速度 ,然后再两个方向上继续进行串级PID控制,最终控制无人机向目标点
飞行。本设计算法水平和竖直两个方向是相互关联而不是各自独立的,而本算计基于的数
学关系的控制效果是水平方向和竖直方向同时到达,即若水平方向到达了水平目标距离,
那竖直方向也必然到达了竖直目标距离,故在飞向目标点的过程中不会撞到障碍物。
记为预定运动距离m,所述m≠0。
点之间的实时距离d=m‑n。
合速度进行几何分解得到水平方向、竖直方向两个目标分速度,由此可保证无人机从实时
位置向目标点的三维空间运动过程中,在水平方向和竖直方向能同时到达目标点,从而避
免因水平方向、竖直方向距离悬殊过大而导致的距离较短的方向先到达目标点、而距离较
长的方向后达到目标点的情形出现,同时避免发生无人机撞击到障碍物的事故发生,以对
无人机实现更为精准的运动控制。
实现如上述实施例提供的所述的无人机三维空间运动控制方法。可以理解,电子设备还可
以包括,输入/输出(I/O)接口,以及通信组件。
用程序或方法的指令,以及应用程序相关的数据。
(Programmable Logic Device,简称PLD)、现场可编程门阵列(Field Programmable Gate
Array,简称FPGA)、控制器、微控制器、微处理器或其他电子元件实现,用于执行上述实施例
提供的所述的无人机三维空间运动控制方法。
读存储器(Electrically Erasable Programmable Read‑Only Memory,简称EEPROM) ,可
擦除可编程只读存储器(Erasable Programmable Read‑Only Memory,简称EPROM) ,可编
程只读存储器(Programmable Read‑Only Memory,简称PROM) ,只读存储器(Read‑Only
Memory,简称ROM),磁存储器,快闪存储器,磁盘或光盘。
元集成在一个单元中。所述功能如果以软件功能单元的形式实现并作为独立的产品销售或
使用时,可以存储在一个计算机可读取存储介质中。
介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设
备等)执行本发明各个实施例所述方法的全部或部分步骤。
程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘、服务器、APP应
用商城等等各种可以存储程序校验码的介质,其上存储有计算机程序,所述计算机程序被
处理器执行时可以实现如下方法步骤:
以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员
可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出
本发明的范围。所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的
系统、装置和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘
述。
划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件
可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或
讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口,装置或单元的间接耦
合或通信连接,可以是电性,机械或其它的形式。所述作为分离部件说明的单元可以是或者
也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以
位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分
或者全部单元来实现本实施例方案的目的。
解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等,如果该特
定姿态发生改变时,则该方向性指示也相应地随之改变。此外,术语“包括”和“具有”以及它
们任何变形,意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系
统、产品或设备没有限定于已列出的步骤或单元,而是可选地还包括没有列出的步骤或单
元,或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。
相同的实施例,也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式
地和隐式地理解的是,本文所描述的实施例可以与其它实施例相结合。
盖在本发明的保护范围之内。因此,本发明的保护范围应以权利要求的保护范围为准。以上
所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,
所作的任何修改、等同替换等,均应包含在本发明的保护范围之内。