一种四旋翼无人机栖停机动轨迹规划、跟踪控制方法

摘要

本发明一种四旋翼无人机栖停机动轨迹规划、跟踪控制方法,属于无人机技术领域；所述轨迹规划、跟踪控制方法包括四旋翼无人机动力学建模、四旋翼无人机栖停机动轨迹规划和轨迹跟踪控制；本发明提供的算法则是采用预先生成的俯仰角序列用于生产初始轨迹，能够极大地降低计算量。最后，该算法还能够根据目标点的运动状态约束对起始点进行运动状态设计，复用率高。本发明的跟踪控制方法，能够提高四旋翼无人机对栖停机动轨迹的跟踪控制收敛速度与鲁棒性，提高四旋翼轨迹跟踪控制的精度。

说明书

技术领域

本发明属于无人机技术领域，具体涉及一种四旋翼无人机栖停机动轨迹规划、跟踪控制方法。

背景技术

微小型四旋翼无人机(Micro Quadrotor Unmanned Aerial Vehicle，简称MQUAV)是当下的研究热点，在军事、民用等领域获得广泛应用。军事领域中，MQUAV装备于班组或单兵，用于战场侦查监视或者打击。民用领域中，MQUAV可以承担区域监控、数据采集、航空拍摄等任务。MQUAV以其低成本、轻小便携、强机动性等优势获得广泛关注，但无人机尺度减小伴随着储能空间急剧减缩，引发了续航时间缩短、使用效能恶化的严重问题。因此，提升MQUAV续航时间成为当下的一个亟待解决的热点问题。

垂面栖停策略是当前解决续航问题的一个有效手段，无人机垂面栖停过程如图1所示。栖停策略的灵感来源于鸟类栖落行为，是通过在无人机上加装模仿动物肢体的栖附装置，赋予无人机在线缆或者建筑物壁面栖附的能力。无人机栖附时仅借助外部作用力克服重力，无需驱动螺旋桨产生推力，因此，可以达到降低能耗、延长有效任务时间的目的。

随着栖停机动应用范围的日益广泛，与之相关的轨迹规划问题也就受到越来越广泛的关注。由于栖停机动过程的约束特征以及四旋翼无人机的欠驱动、强耦合特性，如何规划四旋翼无人机栖停机动轨迹成为本领域技术人员需要解决的重要技术问题。

CN113342056A提供了无人机智能控制技术领域的一种基于改进A星算法的无人机轨迹规划方法，在地图上设定一起始点以及一目标点，基于所述起始点以及目标点进行随机采样得到采样点利用A星算法以及所述节点距离矩阵进行最短路径搜索，完成无人机轨迹规划。CN106774421B公开了一种无人机轨迹规划系统，包括：无人机模块和云平台模块，其中，无人机模块用于采集当前无人机的飞行轨迹信息，并将该飞行轨迹信息上传至云平台模块；云平台模块用于对接收的飞行轨迹信息进行分析，并将对应的最优轨迹规划信息反馈给无人机模块所对应的无人机。CN110308699A公开了一种轨迹规划方法，该方法首先在笛卡儿空间中利用PH曲线平滑竖直运动与水平运动间的直角过渡部分，确定运动轨迹；接着利用多项式运动规律对轨迹的1维曲线位移进行规划，确定运动轨迹插补点的位置；最后以最小化拾放操作周期为目标优化PH曲线参数，并经过推导证明该优化方法能得到PH曲线下最小的曲率差。

现有技术能够根据运动路径开展前端的路径生成(CN113342056A)以及后端的轨迹优化(CN110308699A)，技术中心在于生成满足避障与轨迹光滑连续的运动轨迹。但是，对于栖停问题而言，所生成的轨迹不仅要满足动力学约束，还需要满足过程约束以及末端的速度、角度等约束，因而，该问题不在是单纯的路径规划问题，而应是混合约束条件下的轨迹规划问题。因此，在本发明中提出，采用罚函数方法结合数值优化构建垂面栖停的轨迹规划方法。

CN111413994A公开了一种四旋翼无人机直接自适应模糊控制方法，通过自结构算法对模糊系统进行调整，解决了四旋翼无人机的常规算法中的被控对象内部结构或外部环境发生变化而带来被控对象和数学模型失配、控制效果大打折扣、系统不稳定的不足之处。CN111221346A公开了一种人群搜索算法优化PID控制四旋翼飞行器飞行的方法，基于人群搜索算法优化PID控制参数的控制方法来控制四旋翼飞行器，使得飞行器抗干扰能力得到加强，自身鲁棒性得到提高，在受到外部干扰时能够实时地对飞行器的飞行姿态进行自调整。

现有控制方法均以位置控制为目标，采用位置与姿态的串级控制方法，将位置控制作为外环，姿态控制作为内环，采用传感器感知距离误差，利用距离误差驱动姿态控制回路。这样的控制方法并不适用栖停控制，因为，栖停过程控制的目标既要进行位置与速度控制，还需进行姿态角控制。因而，必须采用以轨迹规划为前提的跟踪控制方法。随着栖停机动应用范围的日益广泛，与之相关的轨迹规划和飞行控制问题也就受到越来越广泛的关注。由于栖停机动过程的约束特征以及四旋翼无人机的欠驱动、强耦合特性，如何对该轨迹进行跟踪控制成为本领域技术人员需要解决的重要技术问题。因此，本发明提供一种轨迹跟踪控制方法，以栖停轨迹为参考输入，建立了适用栖停机动控制的改进几何跟踪控制方法。

发明内容

要解决的技术问题：

为了避免现有技术的不足之处，本发明提出一种四旋翼无人机栖停机动轨迹规划、跟踪控制方法，用以解决四旋翼无人机垂面栖停运动轨迹规划和跟踪控制问题。

本发明的技术方案是：一种四旋翼无人机栖停机动轨迹规划、跟踪控制方法，其特征在于具体步骤如下：

步骤一、四旋翼无人机动力学建模；

首先坐标系选定：四旋翼无人机建模所需的坐D标系，分别是地面坐标系O

然后建立动力学方程，根据刚体运动学方程和动力学方程建立四旋翼无人机的纵向运动的动力学方程：

其中，F

步骤二、四旋翼无人机栖停机动轨迹规划；

2.1栖停机动时间估计；

轨迹无人机栖停机动所需时间T

式中，β

2.2俯仰角时间序列计算；

在步骤2.1得到的四旋翼无人机栖停所需时间T

2.3运动轨迹生成；

根据步骤2.2建立的俯仰角时间变化序列，得到角速度、角加速度相对时间的变化曲线；再结合步骤一建立的动力学模型，设定栖停过程中四旋翼无人机总推力的时间变化序列；得出四旋翼无人机的纵向运动轨迹及运动信息计算公式如下：

式中，U

2.4起始点进行运动状态设计；

根据目标位置与位移，获得起始点位置(x

x

z

其中，Δx为无人机的水平位移，Δz为无人机的垂直位移，(x

步骤三、四旋翼无人机栖停机动轨迹跟踪控制；

3.1设计与步骤一中动力学模型相对应的栖停机动轨迹跟踪控制律，并在软件平台中搭建仿真框架；

所述轨迹跟踪控制律以运动误差为输入，控制算法包括推力总和、力矩和控制分配；

(1)推力总和求解，即无人机的目标推力F为：

F＝f

式中，e

(2)力矩求解公式为：

M＝-k

式中，k

(3)控制分配：

根据推力模型，全机总推力表示为：

式中，f为总推力，ω

无人机机身在坐标系下的x、y、z轴的力矩分别为：

式中，M

3.2将步骤二规划的四旋翼无人机栖停机动的运动轨迹数据加载至步骤3.1的软件平台中进行读取，结合步骤一建立的动力学模型获取当前时刻的无人机的运动状态信息与目标轨迹的运动误差信息，作为当前时刻的误差信息输入；

3.3基于步骤3.2得到的运动误差信息和步骤3.1轨迹跟踪控制算法得到当前时刻的总推力、力矩和，根据控制分配矩阵生成四旋翼的四个电机推力，将该推力输入到步骤一建立的无人机动力学模型，得到四旋翼无人机加速度信息表达式：

四旋翼无人机的纵向运动轨迹及运动信息计算公式如下：

式中，U

3.4重复进行步骤3.2至步骤3.3，对运动轨迹进行持续跟踪。

本发明的进一步技术方案是：所述步骤一中，地面坐标系是固定于地球的惯性坐标系，机体坐标系则是固定于无人机的坐标系，坐标系中心O

本发明的进一步技术方案是：所述步骤二的2.1中，根据四旋翼无人机尺寸L、质量m、转动惯量I、电机最大推力F

(1)电机平衡推力F

(2)电机最大推力F

(3)电机最大俯仰角加速度β

(4)飞行时间T

本发明的进一步技术方案是：所述步骤二的2.4中，根据无人机栖停特点，建立无人机在轨迹末状态的运动状态约束：

x(t＝0)＝x

z(t＝0)＝z

|z

max{F

0.8m/s≤U

-0.2m/s≤W

进一步根据步骤四生成的运动轨迹，对起始点运动状态进行设计如下：

起始点解算是根据目标位置、运动约束以及开环运动轨迹确定起始点位置与运动状态；

假设目标位置坐标为(x

U

W

开环运动轨迹中，无人机的水平位移Δx、垂直位移Δz表示为

Δx＝x(t＝T

Δz＝z(t＝T

开环运动轨迹中，无人机的水平速度变化量ΔU、垂直速度变化量ΔW表示为

ΔU＝U(t＝T

ΔW＝W(t＝T

根据运动约束与速度变化量，可以获得起始点速度U

U

W

根据目标位置与位移，可得起始点位置(x

x

z

本发明的进一步技术方案是：所述步骤二2.4中，所述轨迹末端运动状态约束包括：角速度约束、水平速度约束、垂直速度约束等；所述的初始运动状态包括：初始水平速度、初始垂直速度、初始位置坐标。

本发明的进一步技术方案是：所述步骤三3.1中，仿真框架包括轨迹读取、动力学模型和控制器，其中轨迹读取能够从外部存储中读取无人机的轨迹信息，控制器能够根据无人机运动状态误差信息得到推力和力矩。

本发明的进一步技术方案是：所述步骤三3.1中，推力控制的目的是为了消除无人机相对目标轨迹的位置误差和速度误差，根据误差信号所确定的目标推力矢量f

式中，e

将目标推力矢量f

本发明的进一步技术方案是：所述步骤三3.1中，f

本发明的进一步技术方案是：所述步骤三3.1中，力矩控制的目的是消除无人机相对目标轨迹的角度误差与角速度误差；定义e

e

式中，Ω为角速度矢量，Ω

本发明的进一步技术方案是：所述步骤三3.1中，在MATLAB/Simulink平台中搭建控制设计仿真框架，控制算法和无人机动力学模型都通过MATLAB-Function来编写和实现。

有益效果

本发明的有益效果在于：本发明提供一种四旋翼无人机栖停机动轨迹规划、跟踪控制方法。首先，该方法以动力学模型为基础，兼顾无人机机动能力约束，是动力学可行的飞行轨迹。其次，该方法避免了轨迹规划过程中的搜索与枚举，在CN113342056A中对算法表述为“利用A星算法以及所述节点距离矩阵进行最短路径搜索”，CN112859924A中对算法表述为“通过所述人工噪声结合所述ESO-PSO无人机轨迹预测模型预测无人机轨迹”，由此可见，常规算法在规划过程中往往采用数值优化方法进行搜索与寻优，数值优化过程计算量较大，计算效率较低，本发明提供的算法则是采用预先生成的俯仰角序列用于生产初始轨迹，能够极大地降低计算量。最后，该算法还能够根据目标点的运动状态约束对起始点进行运动状态设计，复用率高。从算法执行过程来看，CN113342056A、CN112859924A等专利所提出的算法在计算参数发生改变时，计算过程几乎需要推倒重来，但是，本发明提供算法由于设定了恒定的中间过程，当末状态约束发生变化时，无需重新开展过程计算，仅需再次进行初始点装配。本发明的轨迹规划结果具有动力学的可行性、数值计算工作量减小、规划效率提高且复用率高的优点。

本发明的跟踪控制方法，能够提高四旋翼无人机对栖停机动轨迹的跟踪控制收敛速度与鲁棒性，提高四旋翼轨迹跟踪控制的精度。仿真试验表明，在给定参考输入前提下，当起始位置存在水平速度摄动(±0.2m/s)、垂直速度摄动(±0.2m/s)或垂直位置摄动(±0.05m)时，跟踪控制算法均能够引导无人机跟踪参考轨迹，当无人机到达目标位置时，俯仰角误差<8°，位置误差<0.02m。

附图说明

图1为四旋翼无人机垂面栖停示意图；

图2为四旋翼无人机动力学建模示意图；

图3为本发明实施例中四旋翼无人机俯仰角时间序列曲线图；

图4为本发明实施例中四旋翼无人机运动轨迹示意图；(1)水平位置时间曲线；(2)垂直位置时间曲线；(3)水平速度时间曲线；(4)垂直速度时间曲线；(5)运动轨迹曲线；

图5为垂面栖停跟踪控制流程图；

图6为本发明实施例中的四旋翼无人机控制设计仿真框架示意图；

图7(a)为本发明实施例中四旋翼无人机栖停机动目标轨迹的水平位移时间曲线；

图7(b)为本发明实施例中四旋翼无人机栖停机动目标轨迹的垂直位移时间曲线；

图7(c)为本发明实施例中四旋翼无人机栖停机动目标轨迹的水平速度时间曲线；

图7(d)为本发明实施例中四旋翼无人机栖停机动目标轨迹的垂直位移时间曲线；

图7(e)为本发明实施例中四旋翼无人机栖停机动目标轨迹的俯仰角时间曲线；

图7(f)为本发明实施例中四旋翼无人机栖停机动目标轨迹曲线；

图8(a)为本发明实施例中栖停机动目标轨迹的水平位移跟踪效果对比曲线；

图8(b)为本发明实施例中栖停机动目标轨迹的垂直位移跟踪效果对比曲线；

图8(c)为本发明实施例中栖停机动目标轨迹的水平速度跟踪效果对比曲线；

图8(d)为本发明实施例中栖停机动目标轨迹的垂直位移跟踪效果对比曲线；

图8(e)为本发明实施例中栖停机动目标轨迹的俯仰角跟踪效果对比曲线；

图8(f)为本发明实施例中栖停机动目标轨迹的跟踪效果对比曲线。

具体实施方式

下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

一种四旋翼无人机栖停机动轨迹规划方法，包括如下步骤：

步骤一、四旋翼无人机动力学建模

本实施例中，选用四旋翼无人机的质量m为0.235kg，转动惯量I为0.002kg·m

四旋翼无人机纵向运动的动力学方程如下所示

I为四旋翼无人机关于y

步骤二、四旋翼无人机栖停机动轨迹规划；

2.1栖停机动时间估计

根据四旋翼无人机参数，估计轨迹无人机栖停机动所需时间T

(1)电机平衡推力F

(2)电机最大推力F

(3)电机最大俯仰角加速度β

(4)飞行时间T

2.2生成俯仰角时间序列

在估计时间T

2.3生成无人机纵向运动轨迹

所述步骤三建立了俯仰角时间变化序列，同时可以获得角速度、角加速度相对时间的变化曲线。根据动力学模型可知，四旋翼无人机的纵向运动由角度和受力两个因素所决定。因此，在开展轨迹优化之前，首先进行运动轨迹生成。假设栖停过程中四旋翼无人机推力之和恒等于重力，则可以获得四旋翼无人机的纵向运动轨迹及运动信息，计算结果如4图。

2.4无人机初始运动状态设计

所述步骤2.3生成的运动轨迹表明，无人机水平向前运动距离为0.895m，垂直向下运动距离为0.15m，水平速度变后量为-2.5m/s，垂直速度变化量为-1.6m/s。设定四旋翼无人机栖停目标位置坐标为(0,0,0)，0.4s时刻的水平速度限定为0.5m/s，0.4s时刻的垂直速度限定范围为0m/s。

初始速度设计：末状态水平速度0.5m/s，无人机减速过程速度变化量为-2.5m/s，因此，初始水平速度为3m/s。末状态垂直速度为0m/s，垂直速度变化量为-1.6m/s，因此，初始垂直速度为1.6m/s。

初始位置设计：无人机水平运动距离为0.895m，因此，初始水平坐标为-0.895m。无人机垂直向下运动距离为0.15m，初始垂直速度为1.6m/s，初始垂直计算式为(0+0.15-1.6*0.4)，初始垂直坐标计算结果为-0.49m。

步骤三、四旋翼无人机栖停机动轨迹跟踪控制；

3.1设计轨迹跟踪控制律与搭建仿真框架

本实施例的控制律所对应的Simulink仿真框架如附图6所示，轨迹跟踪控制律具有如下的技术特征：

(1)推力求解

定义位置误差向量e

e

e

x

式中，k

将目标推力矢量f

F＝f

式中，e

(2)力矩求解

力矩控制的目的是消除无人机相对目标轨迹的角度误差与角速度误差，定义e

e

式中，Ω为角速度矢量，Ω

力矩表达式为M＝-k

(3)控制分配：

根据推力模型，全机总推力表示为：

式中，f为总推力，ω

无人机机身在坐标系下的x、y、z轴的力矩分别为：

式中，M

3.2获取目标轨迹与初始误差信息

本实施例所采用的机动轨迹如附图7(a)至附图7(f)所示，该轨迹持续时间0.4s，起始点坐标为(0,0,0)，初始水平速度为3m/s，初始垂直速度为0m/s。初始俯仰角为0，俯仰角速度为0。

3.3控制输出获取

本实施例设定初始位置为(0,0,0)，误差设置如表1所示。跟踪效果对比如附图8(a)至附图8(f)所示，定量计算数据如表2所示。

表1仿真试验参数表

表2仿真试验参数表

计算结果表明，在初始条件的水平速度、垂直速度、垂直位移摄动情况下，控制算法均能够控制无人机跟踪参考输入轨迹进行飞行。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。