CN110297500A - 一种给定多航路点下无人机的连续航迹规划方法 - Google Patents

Abstract

本发明公开了一种给定多航路点下无人机的连续航迹规划方法，包括如下步骤；S1、获取给定航路点和已知障碍；S2、Dubins离线规划获取通过多航路点的可躲避已知障碍的参考轨迹；S3、预测控制对参考轨迹在线重构得到可抑制负载摆动及躲避未知障碍的最优轨迹；将负载摆动和未知障碍加入目标函数模J＝(Y‑Ref)^TQ(Y‑Ref)+D^TMD+U^TNU，并加入负载摆角及避障等系统约束，该方法可实现无人机对负载摆动的抑制以及对未知障碍的规避。

Description

一种给定多航路点下无人机的连续航迹规划方法

技术领域

本发明涉及属于航迹规划领域，尤其涉及一种给定多航路点下无人机航迹规划算法。

背景技术

四旋翼无人机具有能够垂直起降、定点悬停的优点，成为社会发展中的越来越重要的工具。在给定多航路点时，现有的研究成果通过设计控制器算法或者对航迹进行规划，可以使无人机光滑地通过各航路点，进一步地，当路径中出现已知或未知障碍物时，也可通过一些较为智能使完成自主避障地功能。随着技术的不断发展，无人机也逐渐被应用吊挂运输负载，但是负载在跟随无人机飞行的过程中会由于自身惯性，在无人机通过航路点时的加减速、转向等过程中产生摆动，该摆动会引起无人机的不稳定，甚至造成无人机坠机等安全问题。因此为了保证飞行过程中无人机与负载的安全性与稳定性，必须在无人机带载飞行时抑制负载的摆动。

目前对于抑制负载的摆动的研究主要是基于对控制器的研究，通过选用合适的控制器来实现对负载摆动的抑制。一些学者提出基于负载广义运动思想和反步法相结合的内外环结构的控制策略实现带吊挂负载无人机系统的快速准确跟踪，使系统具有良好的暂态稳定性和鲁棒性。一些学者通过基于视觉伺服输出参考的跟踪控制器抑制负载的摆动，还有一些学者将负载模型与无人机模型进行耦合，在其悬停点处进行线性化处理，设计PID串级控制，抑制由于负载摆动对无人机造成的影响。这些方法虽然也能够很好地抑制运动过程中负载摆动对无人机产生的影响，但需要设计并采用新控制器，且控制器负担较大。

除了对相应控制器的研究，一些学者基于无负载时的航迹规划方法对无人机带载系统进行控制，这种应用方式在实际系统中的控制效果并不显著。也有一些学者通过对无人机运动轨迹的优化抑制负载摆动，如采用强化学习的方法。但是这类方法应用较为复杂，且需要进行额外的信息进行前期学习。

四旋翼无人机带载系统在飞行过程中需要减小因负载摆动而引起的无人机系统的不稳定，重新设计带载系统的控制器有一定的难度，且需要从程序上进行相应的改进，而使用一些智能化算法对无人机带载系统进行航迹规划，增大了无人机的运算负担，在现有主控系统运算能力有限的情况下，难以真正实现。且在无人机航迹规划过程中，通常采用的基于位置追踪方法的多航路点航迹规划所得飞行路径不够连续，在航路点转向处也不够平滑。除此之外，还需要考虑对已知和未知障碍物的躲避。

发明内容

为了解决现有技术存在的问题，本发明提供一种给定多航路点下无人机的连续航迹规划方法，该方法基于预测控制的航迹规划算法，在无人机带载通过多航路点的背景下，设计 Dubins路径为无人机规划航迹，并加入对已知障碍物的躲避，在此基础上，结合预测控制，实现无人机对负载摆动的抑制以及对未知障碍的规避。

针对现有技术问题，本发明采用如下技术方案：

一种给定多航路点下带吊挂负载四旋翼无人机的连续航迹规划方法，包括如下步骤；

S1、获取给定航路点和已知障碍；

S2、使用Dubins路径规划获得参考轨迹；

S3、将负载摆动参数和未知障碍约束参数加入目标函数模型，所述目标函数模型为：

J＝(Y-Ref)^TQ(Y-Ref)+D^TMD+U^TNU

其中，Q、M和N为控制加权矩阵；

S4、借助预测控制方法获得最优轨迹。

所述S3步骤中获得目标函数模型中负载摆动参数：

其中，为在k+i时刻的参考状态，且

式中x_Qr(k+i)，y_Qr(k+i)，z_Qr(k+i)为无人机的期望位置。

所述S3步骤中获得目标函数模型中未知障碍约束参数：

式中表示无人机的预测位置与障碍物的距离。

有益效果

1、本发明可有效减少四旋翼无人机带载飞行中负载的摆动，并能够在给定航路点的情况下规划出一条曲率连续的航迹，从而拓宽四旋翼无人机的应用领域。

2、本发明可以实现在线实时滚动优化，有效地减小负载摆动、躲避环境中障碍物，得到一条在给定多航路点下的满足要求的最优轨迹，为底层控制器设计减轻了负担。

附图说明

图1是本发明的流程图。

图2是本发明的结构示意图。

具体实施方式

下面结合附图对本发明作出进一步说明:

1.总体方案设计

针对给定多航路点下带吊挂负载的四旋翼无人机连续航迹规划问题，本发明所提出方案的主要策略如下：

考虑到通常采用的位置追踪方式存在的诸多弊端，本申请考虑已知障碍物影响，利用 Dubins路径在两航点之间生成CLC路径，以得到一条经过所有给定目标航点的连续光滑轨迹。对于事先未知的障碍物和负载的摆动，需要将未知障碍和摆角约束加入性能指标，借助于预测控制的方法，求解最优控制序列，实现对未知障碍的躲避和对负载摆动的抑制。因此，以生成的Dubins路径为参考轨迹，基于预测控制原理，设计以负载和未知障碍物作为惩罚项的目标函数，来实现给定多航路点下，带吊挂负载四旋翼无人机能够有效抑制负载摆动、并能躲避已知和未知障碍物的连续光滑最优轨迹。方案的整体流程如图1所示，其结构示意如图 2所示。

2.带吊挂负载四旋翼无人机模型描述

本发明研究带吊挂负载的四旋翼无人机的航迹规划问题，根据如下拉格朗日方程可推导出带负载的四旋翼无人机的动力学方程。

带吊挂负载的四旋翼无人机模型，以无人机质心为原点定义机体坐标系A＝[x_A,y_A,z_A]，以负载悬挂点为原点定义负载坐标系l＝[x_l,y_l,z_l]，定义广义坐标q＝[x_Q,y_Q,z_Q,α,β]^T，其中 (x_Q,y_Q,z_Q)表示四旋翼无人机在惯性坐标系下的位置，α表示绳子在X_lO_lZ_l上平面投影与 O_lZ_l轴正向的夹角，β表示绳子与其在X_lO_lZ_l上平面投影的夹角。

为表述方便，文中以C表示cos，S表示sin。则四旋翼无人机与负载系统的总动能表示为：

选取惯性坐标系中的地面作为零势能面，则系统的重力势能为无人机与负载的势能之和，可以表示为：

V＝Mgz_Q+mg(z_Q-lcosαcosβ) (3)

Q_k表示在广义坐标下的广义力，指的是除重力以外，系统在广义坐标上的力。由于研究中忽略阻力以及无人机姿态变化对负载产生的旋转力矩，因此广义力为无人机升力在广义坐标系下的分量。由于在航迹规划中不涉及姿态角，定义无人机升力在三个轴上的分力表达为F_x， F_y，F_z，忽略阻力以及无人机姿态变化对负载产生的旋转力矩，则Q_k表示为如下形式：

Q_k＝[0 0 F_x F_y F_z]^T (4)

由此得到四旋翼无人机与负载系统的非线性动力学模型为：

为了降低航迹规划设计的复杂度，提高在工程中应用的可行性，选择在平衡点处对四旋翼无人机与负载系统模型进行线性化处理：

3给定多航点下Dubins参考路径生成

对于给定多航路点下带吊挂负载四旋翼无人机的航迹规划问题，当无人机以恒定速度飞行时，转弯速度控制是有界的。在这种情况下，最大转弯速率对应于某个最小转弯半径以及等效的最大曲率。对于规定方向下的初始和终止航点，Dubins路径可得出两个定向点的最短路径，这对于四旋翼无人机是可实际飞行的路线。因此可在给定多航路点上，寻找满足一定曲率条件且连接同一平面内所有航点的最短飞行轨迹，本发明中只考虑Dubins路径中的CLC 路径。

首先研究在地图信息已知情况下的航迹规划，假设地图中的障碍物集合为 D＝{D₁,D₂,D₃…D_k}，给定航点设置为P＝{P₁,P₂,P₃…P_f}，则最短可飞行路径为一条绕过n 个障碍物且穿过所有航点的Dubins曲线。

为了能够生成一条光滑曲线，在每个航点处根据飞行速度虚拟出一个以最小转弯半径R 为半径的圆，保证航点在虚拟圆上，无人机在飞行中受到自身最小的转弯半径约束，最小转弯半径R表示如下：

其中，k是与重力加速度有关的常数，最小转弯半径是由重力加速度和速度共同限定的。

在每个航点的虚拟圆半径确定后，需要确定起止圆与终止圆的圆心坐标。假设无人机由航点P_i向P_i+1飞行，航向确定且航点已知，以当前位置为顶点连接前一航点与下一航点构成一个角，则圆心即在这个角的角平分线上，再结合半径便可以确定起始圆和终止圆圆心的位置。令表示沿着角平分线的单位向量，表示虚拟圆圆心坐标，则

在两圆已经确定的情况下，可求得Dubins路径中的直线段。在此假定两个圆的圆心距离足够大，则直线求解问题即转换成两圆上切点的计算问题，它们的公共切线即为两圆之间Dubins路径中的直线部分。在本发明中，两航点之间的航向有两种情况，一种就是在该航点处航线按逆时针方向前行，则直线段为两圆的外公切线；另一种是按顺时针方向前行，则直线段为两圆的内公切线。下面以虚拟圆的顺时针走向为例介绍两切点的求法。

假设两圆的坐标分别为(x₀,y₀)，(x₁,y₁)，则圆心距为：

利用三角函数求出相应的夹角：

则切点与圆心连线与x轴夹角为：

则两切点坐标可求得：

逆时针走向的求解与上述方向类似，由此可求得直线段两切点的坐标。

对于路径上的已知障碍，需要在Dubins路径规划的过程中进行规避。而对于同一障碍物，无人机避障路径可以有两条，通过计算两条路径的长度，对比选取最短的一条作为Dubins避障曲线，进而得到一条光滑的、通过多航路点的、能够避开已知障碍物的航线。

4.基于预测控制的带吊挂负载四旋翼无人机在线航迹规划

通过上述方法，可以得到在给定多航路点情况下可躲避已知障碍的Dubins参考路径，将该路径离散化后可以作为预测控制中的参考轨迹进行优化。但在实际飞行中还可能遇到未知障碍，在检测到飞行路径上的未知障碍物后，将预测位置与该障碍物的距离作为惩罚项加入代价函数和约束，从而在参考轨迹不能避开未知障碍物的情况下，实现对路径上未知障碍的实时在线规避。同时，为了抑制负载的摆动，还需要将对负载摆角的限制加入约束条件，实现在飞行过程中对负载摆动的抑制。

定义状态量选取四旋翼无人机的升力在三轴方向上的分量作为系统的控制量,为预测控制的输出量。将方程(6)整理成状态空间形式，并按照采样周期T进行离散化，得到如下离散状态空间模型，用于建立预测模型，进行航迹规划。

定义如下形式的变量：

其中，为定义在k时刻的控制输入，表示未来时域内的预测输出向量，N_c、 N_p分别表示预测时域和控制时域，且N_c≤N_p，当预测时刻超过N_c时，控制量不发生改变。

假设系统的状态变量在k时刻可测，根据下式可预测未来N_p个时刻的系统状态：

由得到预测输出方程：

其中，是用来描述预测输出与初始时刻状态关系的矩阵，具体表示为：

得到预测模型后，需要建立预测控制性能指标，在设计目标函数时考虑以下条件：

(1)四旋翼无人机的轨迹需接近参考轨迹；

(2)考虑避障要求，以最小安全距离躲避障碍物；

(3)为减小飞行过程中负载的摆动，要求限制负载摆角的最大值；

(4)满足无人机在飞行过程中的物理约束。

因此带吊挂负载四旋翼无人机航迹规划的目标函数可以设计为：

J＝(Y-Ref)^TQ(Y-Ref)+D^TMD+U^TNU (22)

其中，Q、M和N为控制加权矩阵。

在该目标函数中，第一项为预测步内无人机吊挂负载系统的预测状态量与参考状态量的累积误差，包括位置误差以及负载角度误差。目标向量Ref定义为:

其中，为在k+i时刻的参考状态，且

式中x_Qr(k+i)，y_Qr(k+i)，z_Qr(k+i)为无人机的期望位置，在本发明中将其设定为离散的 Dubins路径点；α_r(k+i)，β_r(k+i)，为期望的摆角以及摆角速度。为了能够在预测控制中减小负载的摆动，在设计目标函数时将两个方向的期望摆角及期望摆角速度设为0，以达到减小负载摆动的目的。

目标函数的第二项是当无人机接近障碍物时产生的惩罚项，取为预测位置与障碍物的距离的导数向量，设计该项的目的是当无人机靠近负载时，通过该项的增大使得无人机远离障碍物，从而使得无人机能够有效地躲避障碍，D表示为：

式中表示无人机的预测位置与障碍物的距离。

挂载无人机在飞行过程中还需满足多种约束，包括由无人机自身性能引起的约束，也包括因吊挂负载产生的对最大摆角的限制，因此在求解目标函数过程中要加入下列约束：

i＝1,2…N_p

式(26)的约束条件分别代表了：受无人机动力学限制，要求控制输入u不超过由无人机自身性能所决定的最大值；由于无人机状态可测，在k时刻选取当前系统的实际状态作为初值；要求无人机的速度控制在最大速度范围内；要求负载的摆角小于最大摆角；考虑无人机的避障，在约束中也要求无人机的预测位置与障碍物的距离大于安全距离。

通过求解带有约束的最优问题，可以得到控制器下一时刻的最优控制状态值。在每一个采样时间内，采用非线性优化算法对目标函数求最优解，得到最优控制序列以及预测状态量，选择优化后的位置量作为四旋翼无人机的期望位置，从而得到最优轨迹。

以k时刻为当前时刻，该算法的具体步骤可总结如下：

(1)对优化变量、迭代步数、权重系数等相关参数进行初始化，获取目标航点与地图中的已知障碍物；

(2)根据航点及已知障碍物信息生成Dubins参考路径，得到无人机位置坐标参考值；

(3)根据式(22)生成当前时刻下的最优问题，并求解带约束的最优问题，得到最优控制序列；

(4)将最优控制序列u(k)施加给无人机，可得到无人机与吊挂负载系统k+1时刻的状态量。

为验证本发明所提的预测控制方法在无人机追踪航迹规划问题中的有效性，仿真在内存为4G的普通PC机上进行，在Matlab R2014环境下编程实现算法，选取预测步长M＝5，采样时间为T＝0.1s，控制约束为v_max＝20m/s，α_max＝30°，β_max＝30°。最后得到的仿真结果表明在飞行中摆角α、β均小于最大值，且无人机飞行轨迹连续。本发明的有效性得到成功验证。

应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (3)

1.一种给定多航路点下无人机的连续航迹规划方法，其特征在于，包括如下步骤；

S1、获取给定航路点和已知障碍；

S2、Dubins离线规划获取通过多航路点的可躲避已知障碍的参考轨迹；

S3、预测控制对参考轨迹在线重构得到可抑制负载摆动及躲避未知障碍的最优轨迹；将负载摆动和未知障碍加入目标函数模型，借助预测控制方法获得最优轨迹,所述目标函数模型为：

J＝(Y-Ref)^TQ(Y-Ref)+D^TMD+U^TNU

其中，Q、M和N为控制加权矩阵。

2.根据权利要求1所述的一种给定多航路点下无人机的连续航迹规划方法，其特征在于，所述S3步骤中获得目标函数模型中负载摆动参数：

其中，为在k+i时刻的参考状态，且

式中x_Qr(k+i)，y_Qr(k+i)，z_Qr(k+i)为无人机的期望位置。

3.根据权利要求1所述的一种给定多航路点下无人机的连续航迹规划方法，其特征在于，所述S3步骤中获得目标函数模型中未知障碍约束参数：

式中表示无人机的预测位置与障碍物的距离。